Skip to content
Snippets Groups Projects
Verified Commit 8a05f0ae authored by Thomas Lambert's avatar Thomas Lambert :helicopter:
Browse files

fix(wip): improve control, log frequency

parent ebdce992
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
controller/controller.ino
+ 43
− 34
View file @ 8a05f0ae
......@@ -39,9 +39,11 @@ const uint8_t gCSN_PINS[] = {3,4,5,6}; // Encoder Chip Select pins
const int gMAX_SIG = 2000; // Max signal to the ESC [ms]
const int gMIN_SIG = 1000; // Min signal to the ESC [ms]
const int gMAX_RPM = 3000; // Motor maximum RPM (set in ESC)
const int gMIN_RPM = 750; // Motor minimum RPM (set in ESC)
const int gGEAR_RATIO = 0.1; // Gear ratio between motor and wings
const int gMAX_RPM = 3000; // Motor maximum RPM (set in ESC)
const int gMIN_RPM = 750; // Motor minimum RPM (set in ESC)
const float gGEAR_RATIO = 0.1; // Gear ratio between motor and wings
const int gMAX_ESC_VAL = 160; // Value at which motor is at max RPM
const int gMIN_ESC_VAL = 40; // Min value for ESC to start motor (trial-error)
Servo gEsc1; // ESC for front motor
Servo gEsc2; // ESC for aft motor
......@@ -49,9 +51,6 @@ Servo gEsc2; // ESC for aft motor
int gEsc_val = 0; // ESC angle (to use with the Servo library) [0-180]
String gMode; // Mode of operation
// [TODO]: Delete that
const int gMIN_FREQ = 3000; // Motor maximum RPM (set in ESC)
const int gMAX_FREQ = 3000; // Motor maximum RPM (set in ESC)
/*******************************************************************************
ARDUINO SETUP
......@@ -75,7 +74,6 @@ void setup() {
pinMode(DO_PIN, INPUT);
digitalWrite(CLK_PIN, HIGH);
// Initialize modes
int switch_state = digitalRead(SWITCH_PIN);
if(switch_state == LOW){
......@@ -89,7 +87,7 @@ void setup() {
gMode = "SERIAL";
Serial.println("[WARN]: Control motors using serial");
gEsc_val = setupserial();
gEsc_val = SetupSerial();
}
Serial.println("[INFO]: Setup OK, starting now...");
delay(1000); // Ensure all is properly initialized
......@@ -118,7 +116,7 @@ void loop() {
int old = gEsc_val;
gEsc_val = Serial.parseFloat();
if (gEsc_val == 0){
stopmotor(old);
StopMotor(old);
delay(2000);
exit(0);
}
......@@ -129,7 +127,6 @@ void loop() {
}
else if(gMode == "MANUAL"){
// Read and map potentiometer to use with ESC
gEsc_val = map(potent, 0, 1023, 0, 180);
}
......@@ -142,7 +139,7 @@ void loop() {
wing_angles[2] = (unsigned int)2;
wing_angles[3] = (unsigned int)3;
serialprint(gMode, potent, gEsc_val, wing_angles);
SerialPrint(gMode, potent, gEsc_val, wing_angles);
}
......@@ -153,27 +150,28 @@ void loop() {
// Setup serial
// Get the initial value for the ESCs in serial mode
float setupserial() {
float SetupSerial() {
float input;
float min_freq = RpmToFreq(gMIN_RPM);
float max_freq = RpmToFreq(gMAX_RPM);
Serial.print("[INPUT]: Input a flapping frequency between ");
Serial.print(gMIN_FREQ);
Serial.print(min_freq);
Serial.print(" and ");
Serial.print(gMAX_FREQ);
Serial.print(max_freq);
Serial.println(" [Hz]");
while (input>gMAX_FREQ or input<gMIN_FREQ){
while(Serial.available() == 0)
{
// Loop indefinitely while waiting for serial input
}
while (input>min_freq or input<max_freq){
// Loop indefinitely while waiting for serial input
while(Serial.available() == 0) {}
input = Serial.parseFloat();
if (input > gMAX_FREQ or input < gMIN_FREQ) {
if (input > max_freq or input < min_freq) {
Serial.print("[ERROR]: Frequency must be between ");
Serial.print(gMIN_FREQ);
Serial.print(min_freq);
Serial.print(" and ");
Serial.print(gMAX_FREQ);
Serial.print(max_freq);
Serial.print(" [Hz]. Entered: ");
Serial.println(input);
}
......@@ -182,30 +180,37 @@ float setupserial() {
Serial.print(input);
Serial.println(" Hz.");
return input;
return FreqToEsc(input);
}
// Converts frequence to ESC angle to use with Servo library
int freqtoesc(float freq) {
int ret = 0;
int FreqToEsc(float freq) {
// Need to scale times 100 so we can have better precision
int esc_val = map(freq*100, 0, 100*RpmToFreq(gMAX_RPM), 0, 180);
// Use mapping between freq and 0,180
// Should normally require calibration though...
if (esc_val < gMIN_ESC_VAL) {
esc_val = gMIN_ESC_VAL;
}
return esc_val;
}
return ret;
// Converts RPM to Flapping frequency
float RpmToFreq(int rpm) {
return rpm*gGEAR_RATIO/60.0;
}
// Gracefully stops the motor in serial mode
void stopmotor(int old_esc_val){
void StopMotor(int old_esc_val){
Serial.println("[INFO] MOTOR STOP INITIATED");
if (old_esc_val > 40){
if (old_esc_val > gMIN_ESC_VAL){
Serial.println("[INFO] Slowing down motor for 5 sec...");
gEsc1.write(40);
gEsc2.write(40);
gEsc1.write(gMIN_ESC_VAL);
gEsc2.write(gMIN_ESC_VAL);
delay(5000);
}
......@@ -220,7 +225,7 @@ void stopmotor(int old_esc_val){
//[TODO] FIX THAT
unsigned int readSensor(uint8_t csn){
unsigned int ReadSensor(uint8_t csn){
unsigned int dataOut = 0;
digitalWrite(csn, LOW);
......@@ -241,7 +246,9 @@ unsigned int readSensor(uint8_t csn){
// Output data to use with serial-studio
void serialprint(String mode, int potent, int esc_val, unsigned int wing_angles[]) {
void SerialPrint(String mode, int potent, int esc_val, unsigned int wing_angles[]) {
float freq = map(esc_val, 0, 180, 0, RpmToFreq(gMAX_RPM)*100.0)/100.0;
Serial.print("/*");
Serial.print(mode);
Serial.print(",");
......@@ -250,6 +257,8 @@ void serialprint(String mode, int potent, int esc_val, unsigned int wing_angles[
Serial.print(potent*5.0/1023.0); // Converts pot val into proper tension
Serial.print(",");
Serial.print(esc_val);
Serial.print(",");
Serial.print(freq);
for (int i = 0; i < sizeof(gCSN_PINS); i++){
Serial.print(",");
Serial.print(wing_angles[i]);
......
controller/mechaRaptor.json
+ 18
− 4
View file @ 8a05f0ae
......@@ -65,6 +65,20 @@
"units": "",
"value": "%4",
"widget": "gauge"
},
{
"alarm": 15,
"fft": false,
"fftSamples": 1024,
"graph": false,
"led": false,
"log": false,
"max": 15,
"min": 0,
"title": "Flap. freq.",
"units": "Hz",
"value": "%5",
"widget": "gauge"
}
],
"title": "Inputs",
......@@ -83,7 +97,7 @@
"min": 0,
"title": "Left",
"units": "deg",
"value": "%5",
"value": "%6",
"widget": ""
},
{
......@@ -97,7 +111,7 @@
"min": 0,
"title": "Right",
"units": "deg",
"value": "%6",
"value": "%7",
"widget": ""
}
],
......@@ -117,7 +131,7 @@
"min": 0,
"title": "Left",
"units": "deg",
"value": "%7",
"value": "%8",
"widget": ""
},
{
......@@ -131,7 +145,7 @@
"min": 0,
"title": "Right",
"units": "deg",
"value": "%8",
"value": "%9",
"widget": ""
}
],
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment