controller.ino

/*******************************************************************************
Controller

Controller for the MechaRaptor tandem flapping wing system.

Currently, both ESC are fed the same signal, which means that the two sets of
wings flap at the same frequency. The phase between the two sets will be
adjusted manually by setting their initial positions as desired.
--------------------------------------------------------------------------------
(c) Copyright 2022 University of Liege
Author: Thomas Lambert <t.lambert@uliege.be>
ULiege - Aeroelasticity and Experimental Aerodynamics
MIT License
Repo: https://gitlab.uliege.be/thlamb/mecharaptor-controller
*******************************************************************************/


/*******************************************************************************
  Includes
*******************************************************************************/
#include <Servo.h>  // Control ESC by sending appropriate PWM signal

/*******************************************************************************
  Macros (for pins and fixed components only!)
*******************************************************************************/

// Pins
#define ESC1_PIN 10   // ESC Front motor
#define ESC2_PIN 9    // ESC Aft motor
#define SWITCH_PIN 7  // ON/OFF switch (mode control)
#define POT_PIN A0    // Potentiometer

#define CLK_PIN 2  // Encoder CLK
#define DO_PIN 8   // Encoder Digital Output
#define CSN_PINS  { 3, 4, 5, 6 }  // Encoder Chip Select pins

#define CUR1_PIN A1   // Current measurement for Front motor
#define CUR2_PIN A2   // Current measurement for Aft motor
#define TENS1_PIN A3  // Tension measurement for Front motor
#define TENS2_PIN A4  // Tension measurement for Aft motor

// Values
#define DIV_RES1 9000.0  // Resistance of R1 in the voltage dividers
#define DIV_RES2 1000.0  // Resistance of R2 in the voltage dividers

#define SEN0098_VCC 5        // Supply voltage of the current sensors
#define SEN0098_SENSI 20.0   // [FIXME] VERIFY!
#define SEN0098_VIOUT 0.12   // [FIXME] VERIFY!
#define SEN0098_OFFSET 0.12  // [FIXME] VERIFY!

#define GEAR_RATIO 0.1  // Gear ratio between motor and wings


/*******************************************************************************
  Globals (for things that I can change manually)
*******************************************************************************/

// Constant values that can be tweaked here or in the ESC before run
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 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)

const uint8_t gCSN_PINS[] = CSN_PINS;            // Encoder Chip Select pins
const float gCUR_FACT = SEN0098_SENSI / 1000.0;  // Factor for current measurement
const float gCUR_QOV = SEN0098_VIOUT * SEN0098_VCC;

// Other
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


/*******************************************************************************
  ARDUINO SETUP
*******************************************************************************/
void setup() {
    //Serial.begin(9600); //[DEBUG]: For debugging only
    Serial.begin(115200);  // Need for good precision

    // Init ESCs
    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!");

    // Init switch
    pinMode(SWITCH_PIN, INPUT);

    // Init angle sensors
    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);

        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
}


/*******************************************************************************
  ARDUINO LOOP
    Control the system
      - In serial mode, we impose values based on serial inputs
      - In manual mode, we use the potentiometer
    Acquisition
      - Get values from the 4 rotary encoders
*******************************************************************************/
void loop() {

    int potent = analogRead(POT_PIN);             // Potentiometer value
    unsigned int wing_angles[sizeof(gCSN_PINS)];  // Wing angles (1L, 1R, 2L, 2R)

    if (gMode == "SERIAL" and Serial.available()) {

        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);

    // Read value from rotary encoders
    for (int i = 0; i < sizeof(gCSN_PINS); i++) {
        wing_angles[i] = ReadSensor(gCSN_PINS[i]);
    }

    // Read current and power
    float psu_cur[2], psu_vol[2];
    GetPsu(TENS1_PIN,CUR1_PIN, psu_vol[0], psu_cur[0]);
    GetPsu(TENS2_PIN,CUR2_PIN, psu_vol[1], psu_cur[1]);

    SerialPrint(gMode, potent, gEsc_val, wing_angles, psu_vol, psu_cur);
}


/*******************************************************************************
  OTHER FUNCTIONS
*******************************************************************************/

// 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);
        }
    }
    Serial.print("[INFO]: Frequency correctly set to ");
    Serial.print(input);
    Serial.println(" Hz.");

    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);

    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;
}

// Converts RPM to Flapping frequency
float RpmToFreq(int rpm) {
    return rpm * GEAR_RATIO / 60.0;
}


// Gracefully stops the motor in serial mode
void StopMotor(int old_esc_val) {

    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);
    }

    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) {}
}


//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;
}


// Output data to use with serial-studio
void SerialPrint(String mode, int potent, int esc_val, unsigned int wing_angles[], float psu_vol[], float psu_cur[]) {
    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++) {
        Serial.print(",");
        Serial.print(wing_angles[i]);
    }
    for (int i = 0; i < sizeof(psu_vol); i++) {
        Serial.print(",");
        Serial.print(psu_vol[i]);
        Serial.print(",");
        Serial.print(psu_cur[i]);
    }
    Serial.println("*/");
}

// Measure current consumed by module
float GetCurrent(byte cur_pin) {
    float cur_raw = (5.0 / 1023.0) * analogRead(cur_pin);

    float cur = cur_raw * -gCUR_QOV + 0.007;  //Trimming vale to make current equal to 0 when no current
    return cur / gCUR_FACT;                   // get actual current
}

// Measure the tension provided by the PSU
float GetTension(byte vol_pin) {
    int vol_raw = analogRead(vol_pin) * (5.0 / 1023.0);

    return vol_raw * (DIV_RES1 + DIV_RES2) / (DIV_RES2);
}

// Get the tension and current of a PSU
void GetPsu(byte vol_pin, byte cur_pin, float &vol, float &cur) {
    vol = GetTension(vol_pin);
    cur=  GetCurrent(cur_pin);

}