fix(wip): proper angle reading

controller/controller.ino

@@ -48,7 +48,7 @@ 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
 
-int gEsc_val = 0;   // ESC angle (to use with the Servo library) [0-180]
+int gEsc_val = 0;  // ESC angle (to use with the Servo library) [0-180]
 String gMode;      // Mode of operation
 
 
@@ -56,7 +56,8 @@ String gMode;      // Mode of operation
   ARDUINO SETUP
 *******************************************************************************/
 void setup() {
-  Serial.begin(9600); //115200 for proper angle measurements, 9600 otherwise
+  //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
@@ -66,13 +67,15 @@ void setup() {
 
   pinMode(SWITCH_PIN, INPUT);
 
-  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);
+
+  // [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);
@@ -107,37 +110,28 @@ void loop() {
   int potent = analogRead(A0); // Potentiometer value
   unsigned int wing_angles[4];   // Wing angles (1L, 1R, 2L, 2R)
 
-  if(gMode =="SERIAL"){
-   /* If nothing new, just continue
-   * If serial input, change value
-   * If serial is 0, stop
-   */
-    if (Serial.available()){
-      int old = gEsc_val;
-      gEsc_val = Serial.parseFloat();
-      if (gEsc_val == 0){
-        StopMotor(old);
-        delay(2000);
-        exit(0);
-      }
-    }
+  if(gMode =="SERIAL" and Serial.available()){
 
-    //[FIXME] Get value properly
-    gEsc_val = 40;
+    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);
   }
 
+  // Write value to ESCs
   gEsc1.write(gEsc_val);
   gEsc2.write(gEsc_val);
 
-  // [FIXME] Use readsensors
-  wing_angles[0] = (unsigned int)0;
-  wing_angles[1] = (unsigned int)1;
-  wing_angles[2] = (unsigned int)2;
-  wing_angles[3] = (unsigned int)3;
+  // 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);
 }
@@ -147,7 +141,6 @@ void loop() {
   OTHER FUNCTIONS
 *******************************************************************************/
 
-
 // Setup serial
 // Get the initial value for the ESCs in serial mode
 float SetupSerial() {
@@ -162,9 +155,9 @@ float SetupSerial() {
   Serial.print(max_freq);
   Serial.println(" [Hz]");
 
-  while (input>min_freq or input<max_freq){
+  while (input>max_freq or input<min_freq){
     // Loop indefinitely while waiting for serial input
-    while(Serial.available() == 0) {}
+    while(Serial.available() == 0) { }
 
     input = Serial.parseFloat();
     if (input > max_freq or input < min_freq) {
@@ -192,7 +185,10 @@ int FreqToEsc(float freq) {
 
   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;
 }
 
@@ -224,9 +220,9 @@ void StopMotor(int old_esc_val){
 }
 
 
-//[TODO] FIX THAT
+//Read 12-bit rotary encoder (RobinL modified by linarism on forum.arduino.cc)
 unsigned int ReadSensor(uint8_t csn){
-  unsigned int dataOut = 0;
+  unsigned int ret = 0;
 
   digitalWrite(csn, LOW);
   delayMicroseconds(1); //Waiting for Tclkfe
@@ -237,11 +233,11 @@ unsigned int ReadSensor(uint8_t csn){
     delayMicroseconds(1); //Tclk/2
     digitalWrite(CLK_PIN, HIGH);
     delayMicroseconds(1); //Tdo valid, like Tclk/2
-    dataOut = (dataOut << 1) | digitalRead(DO_PIN); //shift all the entering data to the left and past the pin state to it. 1e bit is MSB
+    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 dataOut;
+  return ret;
 }
 
 
@@ -258,7 +254,7 @@ void SerialPrint(String mode, int potent, int esc_val, unsigned int wing_angles[
   Serial.print(",");
   Serial.print(esc_val);
   Serial.print(",");
-  Serial.print(freq);
+  Serial.print(freq); // [FIXME] Is it useful?
   for (int i = 0; i < sizeof(gCSN_PINS); i++){
     Serial.print(",");
     Serial.print(wing_angles[i]);

controller/mechaRaptor.json

@@ -75,7 +75,7 @@
                     "log": false,
                     "max": 15,
                     "min": 0,
-                    "title": "Flap. freq.",
+                    "title": "Desired freq.",
                     "units": "Hz",
                     "value": "%5",
                     "widget": "gauge"