diff --git a/controller/controller.ino b/controller/controller.ino
index e9fb0b0d8b89ea3926a028f234d1064b42098e6e..cf4d79e6bb206f05d107136fdbd3aa8d2e3cddbb 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("*/");
}