From 6beeca72ce3169e8517421402eb1b590f7b4400c Mon Sep 17 00:00:00 2001 From: Thomas Lambert <t.lambert@uliege.be> Date: Fri, 4 Nov 2022 00:53:53 +0100 Subject: [PATCH] style: astyle formatter --- controller/controller.ino | 303 +++++++++++++++++++------------------- 1 file changed, 151 insertions(+), 152 deletions(-) diff --git a/controller/controller.ino b/controller/controller.ino index e9fb0b0..cf4d79e 100644 --- a/controller/controller.ino +++ b/controller/controller.ino @@ -18,26 +18,26 @@ Repo: https://gitlab.uliege.be/thlamb/mecharaptor-controller /******************************************************************************* Includes *******************************************************************************/ -#include <Servo.h> // Control ESC by sending appropriate PWM signal +#include <Servo.h> // Control ESC by sending appropriate PWM signal /******************************************************************************* Macros (use for pins only) *******************************************************************************/ -#define ESC1_PIN 10 // ESC Front motor -#define ESC2_PIN 9 // ESC Aft motor -#define SWITCH_PIN 7 // ON/OFF switch (mode control) -#define CLK_PIN 2 // Encoder CLK -#define DO_PIN 8 // Encoder Digital Output +#define ESC1_PIN 10 // ESC Front motor +#define ESC2_PIN 9 // ESC Aft motor +#define SWITCH_PIN 7 // ON/OFF switch (mode control) +#define CLK_PIN 2 // Encoder CLK +#define DO_PIN 8 // Encoder Digital Output /******************************************************************************* Globals *******************************************************************************/ -const uint8_t gCSN_PINS[] = {3,4,5,6}; // Encoder Chip Select pins +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_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) @@ -45,8 +45,8 @@ 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 +Servo gEsc1; // ESC for front motor +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 @@ -56,44 +56,44 @@ String gMode; // Mode of operation ARDUINO SETUP *******************************************************************************/ void setup() { - //Serial.begin(9600); //[DEBUG]: For debugging only - Serial.begin(115200); // Need for good precision - - Serial.print("[INFO]: Attaching ESCs ... "); - delay(5000); // Prevents motor from starting automatically with serial - gEsc1.attach(ESC1_PIN, gMIN_SIG, gMAX_SIG); - gEsc2.attach(ESC2_PIN, gMIN_SIG, gMAX_SIG); - Serial.println("done!"); - - pinMode(SWITCH_PIN, INPUT); - - - // [DEBUG] Comment all that as long as PCB not properly soldered - //for (int i = 0; i < sizeof(gCSN_PINS); i++) { - // pinMode(gCSN_PINS[i], OUTPUT); - // digitalWrite(gCSN_PINS[i], HIGH); - //} - //pinMode(CLK_PIN, OUTPUT); - //pinMode(DO_PIN, INPUT); - //digitalWrite(CLK_PIN, HIGH); - - // Initialize modes - int switch_state = digitalRead(SWITCH_PIN); - if(switch_state == LOW){ - gMode = "MANUAL"; - Serial.println("[WARN]: Control motors using potentiometer"); - - } else{ - gEsc1.write(0); // Must be initialized at 0 otherwise will not start - gEsc2.write(0); + //Serial.begin(9600); //[DEBUG]: For debugging only + Serial.begin(115200); // Need for good precision + + Serial.print("[INFO]: Attaching ESCs ... "); + delay(5000); // Prevents motor from starting automatically with serial + gEsc1.attach(ESC1_PIN, gMIN_SIG, gMAX_SIG); + gEsc2.attach(ESC2_PIN, gMIN_SIG, gMAX_SIG); + Serial.println("done!"); + + pinMode(SWITCH_PIN, INPUT); + + + // [DEBUG] Comment all that as long as PCB not properly soldered + //for (int i = 0; i < sizeof(gCSN_PINS); i++) { + // pinMode(gCSN_PINS[i], OUTPUT); + // digitalWrite(gCSN_PINS[i], HIGH); + //} + //pinMode(CLK_PIN, OUTPUT); + //pinMode(DO_PIN, INPUT); + //digitalWrite(CLK_PIN, HIGH); + + // Initialize modes + int switch_state = digitalRead(SWITCH_PIN); + if (switch_state == LOW) { + gMode = "MANUAL"; + Serial.println("[WARN]: Control motors using potentiometer"); - gMode = "SERIAL"; - Serial.println("[WARN]: Control motors using serial"); + } else { + gEsc1.write(0); // Must be initialized at 0 otherwise will not start + gEsc2.write(0); + + gMode = "SERIAL"; + Serial.println("[WARN]: Control motors using serial"); - gEsc_val = SetupSerial(); - } - Serial.println("[INFO]: Setup OK, starting now..."); - delay(1000); // Ensure all is properly initialized + gEsc_val = SetupSerial(); + } + Serial.println("[INFO]: Setup OK, starting now..."); + delay(1000); // Ensure all is properly initialized } @@ -107,33 +107,32 @@ void setup() { *******************************************************************************/ void loop() { - int potent = analogRead(A0); // Potentiometer value - unsigned int wing_angles[4]; // Wing angles (1L, 1R, 2L, 2R) + int potent = analogRead(A0); // Potentiometer value + unsigned int wing_angles[4]; // Wing angles (1L, 1R, 2L, 2R) - if(gMode =="SERIAL" and Serial.available()){ + if (gMode == "SERIAL" and Serial.available()) { - float input = Serial.parseFloat(); - if (input == 0){ - StopMotor(gEsc_val); - } else { - gEsc_val = FreqToEsc(input); - } + float input = Serial.parseFloat(); + if (input == 0) { + StopMotor(gEsc_val); + } else { + gEsc_val = FreqToEsc(input); + } - } - else if(gMode == "MANUAL"){ - gEsc_val = map(potent, 0, 1023, 0, 180); - } + } else if (gMode == "MANUAL") { + gEsc_val = map(potent, 0, 1023, 0, 180); + } - // Write value to ESCs - gEsc1.write(gEsc_val); - gEsc2.write(gEsc_val); + // Write value to ESCs + gEsc1.write(gEsc_val); + gEsc2.write(gEsc_val); - // Read value from rotary encoders - for (int i = 0; i < sizeof(gCSN_PINS); i++){ - wing_angles[i] = ReadSensor(gCSN_PINS[i]); - } + // Read value from rotary encoders + for (int i = 0; i < sizeof(gCSN_PINS); i++) { + wing_angles[i] = ReadSensor(gCSN_PINS[i]); + } - SerialPrint(gMode, potent, gEsc_val, wing_angles); + SerialPrint(gMode, potent, gEsc_val, wing_angles); } @@ -144,120 +143,120 @@ void loop() { // Setup serial // Get the initial value for the ESCs in serial mode 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(min_freq); - Serial.print(" and "); - Serial.print(max_freq); - Serial.println(" [Hz]"); - - while (input>max_freq or input<min_freq){ - // Loop indefinitely while waiting for serial input - while(Serial.available() == 0) { } - - input = Serial.parseFloat(); - if (input > max_freq or input < min_freq) { - Serial.print("[ERROR]: Frequency must be between "); - Serial.print(min_freq); - Serial.print(" and "); - Serial.print(max_freq); - Serial.print(" [Hz]. Entered: "); - Serial.println(input); + float input; + + float min_freq = RpmToFreq(gMIN_RPM); + float max_freq = RpmToFreq(gMAX_RPM); + + Serial.print("[INPUT]: Input a flapping frequency between "); + Serial.print(min_freq); + Serial.print(" and "); + Serial.print(max_freq); + Serial.println(" [Hz]"); + + while (input > max_freq or input < min_freq) { + // Loop indefinitely while waiting for serial input + while (Serial.available() == 0) {} + + input = Serial.parseFloat(); + if (input > max_freq or input < min_freq) { + Serial.print("[ERROR]: Frequency must be between "); + Serial.print(min_freq); + Serial.print(" and "); + Serial.print(max_freq); + Serial.print(" [Hz]. Entered: "); + Serial.println(input); + } } - } - Serial.print("[INFO]: Frequency correctly set to "); - Serial.print(input); - Serial.println(" Hz."); + Serial.print("[INFO]: Frequency correctly set to "); + Serial.print(input); + Serial.println(" Hz."); - return FreqToEsc(input); + return FreqToEsc(input); } // Converts frequence to ESC angle to use with Servo library 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); + // Need to scale times 100 so we can have better precision + int esc_val = map(freq * 100, 0, 100 * RpmToFreq(gMAX_RPM), 0, 180); - if (esc_val < gMIN_ESC_VAL) { - esc_val = gMIN_ESC_VAL; - } else if (esc_val > gMAX_ESC_VAL) { - esc_val = gMAX_ESC_VAL; - } + if (esc_val < gMIN_ESC_VAL) { + esc_val = gMIN_ESC_VAL; + } else if (esc_val > gMAX_ESC_VAL) { + esc_val = gMAX_ESC_VAL; + } - return esc_val; + return esc_val; } // Converts RPM to Flapping frequency float RpmToFreq(int rpm) { - return rpm*gGEAR_RATIO/60.0; + 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"); + Serial.println("[INFO] MOTOR STOP INITIATED"); - if (old_esc_val > gMIN_ESC_VAL){ - Serial.println("[INFO] Slowing down motor for 5 sec..."); - gEsc1.write(gMIN_ESC_VAL); - gEsc2.write(gMIN_ESC_VAL); - delay(5000); - } + if (old_esc_val > gMIN_ESC_VAL) { + Serial.println("[INFO] Slowing down motor for 5 sec..."); + gEsc1.write(gMIN_ESC_VAL); + gEsc2.write(gMIN_ESC_VAL); + delay(5000); + } - gEsc1.write(0); - gEsc2.write(0); - Serial.println("[INFO] Motor stopped completely"); + gEsc1.write(0); + gEsc2.write(0); + Serial.println("[INFO] Motor stopped completely"); - // Lock system in infinite loop to prevent any restart - delay(1000); - while(1); + // Lock system in infinite loop to prevent any restart + delay(1000); + while (1) {} } //Read 12-bit rotary encoder (RobinL modified by linarism on forum.arduino.cc) -unsigned int ReadSensor(uint8_t csn){ - unsigned int ret = 0; - - digitalWrite(csn, LOW); - delayMicroseconds(1); //Waiting for Tclkfe - - //Passing 12 times, from 0 to 11 - for(int x=0; x<12; x++){ - digitalWrite(CLK_PIN, LOW); - delayMicroseconds(1); //Tclk/2 - digitalWrite(CLK_PIN, HIGH); - delayMicroseconds(1); //Tdo valid, like Tclk/2 - ret = (ret << 1) | digitalRead(DO_PIN); //shift all the entering data to the left and past the pin state to it. 1e bit is MSB - } - - digitalWrite(csn, HIGH); //deselects the encoder from reading - return ret; +unsigned int ReadSensor(uint8_t csn) { + unsigned int ret = 0; + + digitalWrite(csn, LOW); + delayMicroseconds(1); //Waiting for Tclkfe + + //Passing 12 times, from 0 to 11 + for (int x = 0; x < 12; x++) { + digitalWrite(CLK_PIN, LOW); + delayMicroseconds(1); //Tclk/2 + digitalWrite(CLK_PIN, HIGH); + delayMicroseconds(1); //Tdo valid, like Tclk/2 + ret = (ret << 1) | digitalRead(DO_PIN); //shift all the entering data to the left and past the pin state to it. 1e bit is MSB + } + + digitalWrite(csn, HIGH); //deselects the encoder from reading + return ret; } // Output data to use with serial-studio 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(","); - Serial.print(millis()/1000.0); - Serial.print(","); - Serial.print(potent*5.0/1023.0); // Converts pot val into proper tension - Serial.print(","); - Serial.print(esc_val); - Serial.print(","); - Serial.print(freq); // [FIXME] Is it useful? - for (int i = 0; i < sizeof(gCSN_PINS); i++){ + float freq = map(esc_val, 0, 180, 0, RpmToFreq(gMAX_RPM) * 100.0) / 100.0; + + Serial.print("/*"); + Serial.print(mode); + Serial.print(","); + Serial.print(millis() / 1000.0); + Serial.print(","); + Serial.print(potent * 5.0 / 1023.0); // Converts pot val into proper tension Serial.print(","); - Serial.print(wing_angles[i]); - } - Serial.println("*/"); + Serial.print(esc_val); + Serial.print(","); + Serial.print(freq); // [FIXME] Is it useful? + for (int i = 0; i < sizeof(gCSN_PINS); i++) { + Serial.print(","); + Serial.print(wing_angles[i]); + } + Serial.println("*/"); } -- GitLab