File content as of revision 33:db4fc2f642f0:

#include "mbed.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
#include "QEI.h"

DigitalOut led_g(LED_GREEN);
DigitalOut led_b(LED_BLUE);
DigitalOut led_r(LED_RED);

DigitalOut M1_Rotate(D2); // voltage only base rotation
PwmOut M1_Speed(D3);      // voltage only base rotation

MODSERIAL pc(USBTX, USBRX);

//QEI wheel(PinName channelA, PinName channelB, PinName index, int pulsesPerRev, Encoding encoding=X2_ENCODING)
QEI motor2(D10,D11,NC,8400,QEI::X4_ENCODING);
QEI motor3(D12,D13,NC,8400,QEI::X4_ENCODING);

//Motor control outputs
DigitalOut M2_Rotate(D4);   // encoder side pot 2 translation
PwmOut M2_Speed(D5);        // encoder side pot 2 translation
DigitalOut M3_Rotate(D7);   // encoder side pot 1 spatel rotation
PwmOut M3_Speed(D6);        // encoder side pot 1 spatel rotation

//Booleans for simplifying the EMG control
bool left;
bool right;

//EMG related inputs and outputs
HIDScope    scope( 2 );
AnalogIn    emg0( A0 );
AnalogIn    emg1( A1 );
DigitalIn   buttonCalibrate(SW3);
DigitalIn   buttonCalibrateComplete(SW2);

//Variables for control
bool turnRight;
bool turnLeft;
bool turn = 0;
float waiter = 0.12;
float translation = 0; //initialise values at 0
float degrees3 = 0;

float Puls_degree = (8400/360);
float wheel1 = 16;
float wheel2 = 31;
float wheel3 = 41;
float transmission = ((wheel2/wheel1)*(wheel3/wheel1));
float pi = 3.14159265359;

//Global filter variables
volatile float x;
volatile float x_prev =0;
volatile float b; // filtered 'output' of ReadAnalogInAndFilter
const double a1 = -1.6475;
const double a2 = 0.7009;
const double b0 = 0.8371;
const double b1 = -1.6742;
const double b2 = 0.8371;
const double c1 = -1.9645;
const double c2 = 0.9651;
const double d0 = 0.0001551;
const double d1 = 0.0003103;
const double d2 = 0.0001551;
double v1_HR = 0;
double v2_HR = 0;
double v1_LR = 0;
double v2_LR = 0;
double v1_HL = 0;
double v2_HL = 0;
double v1_LL = 0;
double v2_LL = 0;
double highpassFilterLeft = 0;
double lowpassFilterLeft = 0;
double highpassFilterRight = 0;
double lowpassFilterRight = 0;

//calibration
bool calibrate = false;
bool calibrate_complete = false;
double threshold_Left = 0;
double threshold_Right= 0;


//Tickers
Ticker      sample_timer;
Ticker      printinfo;
Ticker      checkSetpointTranslation;
Ticker      checkSetpointRotation;

//LED for testing the code
DigitalOut  led(LED1);


//setpoints
volatile float setpointRotation;
volatile float setpointTranslation;
const double Setpoint_Translation = -300;
const double Setpoint_Back = 0;
const double Setpoint_Rotation = pi;
double M3_ControlSpeed = 0;
double M2_ControlSpeed = 0;
double SetpointError_Translation = 0;
double SetpointError_Rotation = 0;
double theta_translation;
double theta_rotation;

//Variables for storing previous setpoints (for calculation average setpoint over 6 previously measured values)
int counter = 0;
double Setpoint1 = 0;
double Setpoint2 = 0;
double Setpoint3 = 0;
double Setpoint4 = 0;
double Setpoint5 = 0;
double SetpointAvg = 0;

//booleans acting as 'Go-Flags'
bool booltranslate = false;
bool boolrotate = false;

//Arm PID
const double Ts = 0.001953125; //Ts=1/fs (sample frequency)
const double Translation_Kp = 6.9, Translation_Ki = 0.8, Translation_Kd = 0.4;
double Translation_error = 0;
double Translation_e_prev = 0;

//Spatel PID
const double Rotation_Kp = 0.23, Rotation_Ki = 0.0429 , Rotation_Kd = 2;
double Rotation_error = 0;
double Rotation_e_prev = 0;

//Pid calculation (reusable)
double pid_control(double error, const double kp, const double ki, const double kd, double &e_int, double &e_prev)
{
    double e_der = (error - e_prev) / Ts;
    e_prev = error;
    e_int = e_int + (Ts * error);

    return kp*error + ki + e_int + kd + e_der;
}

//biquad calculation (reusable)
double biquad(double u, double&v1, double&v2, const double a1, const double a2, const double b0,
              const double b1, const double b2)
{
    double v = u - a1*v1 - a2*v2;
    double y = b0*v + b1*v1 + b2*v2;
    v2 = v1;
    v1 = v;
    return y;
}

//sample function, samples and processes through a highpassfilter, rectifier and lowpassfilter.
void filterSample()
{
    highpassFilterLeft = fabs(biquad(emg0.read(), v1_HL, v2_HL, a1, a2, b0, b1, b2));
    lowpassFilterLeft = biquad(highpassFilterLeft, v1_LL, v2_LL, c1, c2, d0, d1, d2);
    //pc.printf("%f \n \r ", lowpassFilter);
    highpassFilterRight = fabs(biquad(emg1.read(), v1_HR, v2_HR, a1, a2, b0, b1, b2));
    lowpassFilterRight = biquad(highpassFilterRight, v1_LR, v2_LR, c1, c2, d0, d1, d2);
    scope.set(0, lowpassFilterLeft );
    scope.set(1, lowpassFilterRight );
    scope.send();
    //pc.printf("%f \n \r ", lowpassFilter);
}

//Getting the positions of moving parts by processing the motor angles
float GetPositionM2()
{
    float pulses2 = motor2.getPulses();
    float degrees2 = (pulses2/Puls_degree);
    float radians2 = (degrees2/360)*2*pi;
    float translation = ((radians2/transmission)*32.25);

    return translation;
}
float GetRotationM3()
{
    float pulses3 = motor3.getPulses();
    float degrees3 = (pulses3/Puls_degree);
    float radians3 = (degrees3/360)*2*pi;

    return radians3;
}

// check the error at current sample, and every 50 samples sample for an average (to be used in the controllers)
void CheckErrorRotation()
{
    counter++;
    if (counter > 50) {
        theta_rotation = GetRotationM3();
        Setpoint5 = Setpoint4;
        Setpoint4 = Setpoint3;
        Setpoint3 = Setpoint2;
        Setpoint2 = Setpoint1;
        Setpoint1 = SetpointError_Rotation;
        counter = 0;
    }
    SetpointError_Rotation =  setpointRotation -theta_rotation;

    SetpointAvg = ((SetpointError_Rotation + Setpoint1 + Setpoint2 + Setpoint3 + Setpoint4 + Setpoint5)/6);

}
// check the error at current sample (Translation does well enough without averaging
void CheckErrorTranslation()
{
    theta_translation = GetPositionM2();
    SetpointError_Translation =  setpointTranslation -theta_translation;
}

//Controller function for the rotation of the spatula
void motorRotation()
{
    printf("setpoint = %f \n\r", setpointRotation);
    //set direction
    if (SetpointError_Rotation > 0) { //if rotation exceeds the setpoint, turn in the other direction
        M3_Rotate = 0;
    } else {
        M3_Rotate = 1;

    }
    double speedfactor = 1;
    //when on the way back (Setpoint is not pi, but 0) go at a lower speed for higher accuracy
    if (setpointRotation != Setpoint_Rotation) {
        speedfactor = 0.3; 
    }
    //the way back has to be more precise. On the way up it doesn't really matter at which point the spatula stops. (as long as it's somewhere high)
    double tolerance = 0.1;
    if (setpointRotation == Setpoint_Rotation){
        tolerance = 1;   
    }   
    
    //control action with 'speedfactor' 
    M3_ControlSpeed = speedfactor * Ts * fabs( pid_control(SetpointError_Rotation, Rotation_Kp, Rotation_Ki, Rotation_Kd, Rotation_error, Rotation_e_prev));
    if (fabs(SetpointAvg) < 0.1) { //when average error over the last samples is low enough, stop the motor
        M3_ControlSpeed = 0;
    }
    if (theta_rotation > tolerance) //if the angle is within tolerance, give the correct Go-Flag.
        boolrotate = true;
    if ((fabs(theta_rotation) < tolerance ) && (M3_ControlSpeed == 0))
        boolrotate = false;
    M3_Speed = M3_ControlSpeed;
}

//controller for the translation, similar to the rotation but easier because it does not use speedfactors and variable tolerances et cetera
void motorTranslation()
{
    theta_translation = GetPositionM2();
    SetpointError_Translation =  setpointTranslation - theta_translation;

    //set direction
    if (SetpointError_Translation < 0) {
        M2_Rotate = 0;
    } else {
        M2_Rotate = 1;
    }
    M2_ControlSpeed = Ts * fabs( pid_control(SetpointError_Translation, Translation_Kp, Translation_Ki, Translation_Kd, Translation_error, Translation_e_prev));
    if (fabs(SetpointError_Translation) < 8) {
        M2_ControlSpeed = 0;

    }
    if ((theta_translation < -292) && (M2_ControlSpeed == 0))
        booltranslate = true;
    if ((theta_translation > -8) && (M2_ControlSpeed == 0))
        booltranslate = false;
    M2_Speed = M2_ControlSpeed;

}
//from now on the position of motors can be easily controlled by calling the previous two functions and changing the setpoints.



//go back, so the setpoint will change to Setpoint_Back which is 0
void GoBack()
{
    setpointTranslation = Setpoint_Back; //setting the setpoint
    motorTranslation(); //executing the action
    if (booltranslate == false) {
        setpointRotation = Setpoint_Back;
        motorRotation();
    }
    if (boolrotate == false) {
        turn = 0;
    }
    led_r = 1;
    led_b = 0;
}

//Same as GoBack but forward
void Burgerflip()
{
    led_r = 0;
    led_b = 1;
    setpointTranslation = Setpoint_Translation;
    motorTranslation();
    if (booltranslate == true) { //this is a Go-Flag, for when the translation is complete. When this is the case the spatula can rotate.
        setpointRotation = Setpoint_Rotation; //setting the setpoint
        motorRotation(); //executing the action
    }
}
void BurgerflipActie() //a simple function which calls on the previous two, two perform the 2 actions after each other
{
    Burgerflip();
    if (boolrotate == true) {
        GoBack();
    }
}
void print() //a print function which can be attached to a ticker to read out the positions
{
    pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
}


//Getting the directions. This means reading out the input and setting the correct booleans which are used to control the motors
void GetDirections()
{
    
    //booleans based on EMG input
    if (lowpassFilterRight < threshold_Right)
        right = 0;
    if (lowpassFilterRight > threshold_Right)
        right = 1;
    if (lowpassFilterLeft < threshold_Left)
        left = 0;
    if (lowpassFilterLeft > threshold_Left)
        left = 1;
    pc.baud(115200);
    
    //based on the EMG inputs and the boolean 'turn' (which is a go flag for the burger flip action) turnLeft and turnRight are set.
    //turnLeft and turnRight control the base motor which rotates the entire robot.
    if ((right == 1) && (left == 1) && (turn == 0)) {
        turnLeft = 0;
        turnRight = 0;
        turn = 1;
        pc.printf("start action \n \r ");
        wait(waiter);

    } else if ((right == 1) && (left == 1) && (turn == 1)) {
        turnLeft = 0;
        turnRight = 0;
        turn = 0;
        pc.printf("cancel action \n \r ");
        GoBack();
        wait(waiter);
    } else if ((right == 0) && (left == 0)&& (turn == 0)) {

    } else if ((right == 1) && (turnLeft == 0)&& (turn == 0)) {
        /* if the right button is pressed and the motor isn't rotating to the left,
        then start rotating to the right etc*/
        turnRight = !turnRight;
        pc.printf("turn right \n \r ");
        wait(waiter);
    } else if ((right == 1) && (turnLeft == 1)&& (turn == 0)) {
        turnLeft = 0;
        turnRight = !turnRight;
        pc.printf("turn right after left \n \r ");
        wait(waiter);
    } else if ((left == 1) && (turnRight == 0)&& (turn == 0)) {
        turnLeft = !turnLeft;
        pc.printf("turn left \n \r ");
        wait(waiter);
    } else if ((left == 1) && (turnRight == 1) && (turn == 0)) {
        turnRight = 0;
        turnLeft = !turnLeft;
        pc.printf("turn left after right \n \r ");
        wait(waiter);
    }
    wait(2*waiter);
}
int main()
{
    //setting leds
    led_g = 1;
    led_b = 1;
    led_r = 1;

    /**Attach the 'sample' function to the timer 'sample_timer'.
    * this ensures that 'sample' is executed every... 0.001953125 seconds = 512 Hz
    */
    //sample_timer.attach(&sample, 0.001953125);
    sample_timer.attach(&filterSample, Ts);
    checkSetpointTranslation.attach(&CheckErrorTranslation,Ts);
    checkSetpointRotation.attach(&CheckErrorRotation,Ts);

    //printinfo.attach(&print, Ts);
    pc.baud(115200);
    pc.printf("please push the button to calibrate \n \r");
    while (1) {
        
        //The main function starts with some calibration steps
        if (buttonCalibrate == 0) {
            calibrate = true;
            threshold_Left = lowpassFilterLeft*0.9;  //at the moment of the button press, the current EMG values are stored and multiplied by 0.9 as a threshold value.
            threshold_Right = lowpassFilterRight*0.9;
            pc.printf("calibration complete, press to continue \n \r");
        }
        if ((buttonCalibrateComplete == 0) && (calibrate == true)) {
            calibrate_complete = true;
        }
        if (calibrate_complete == true) {
            
            //After calibrating, the booleans turn, turnRight and turnLeft are evaluated and based on that the motors are controlled.

            pc.printf("rotation is %f, setpoint %f, error = %f en translation = %f en de error %f \n \r", GetRotationM3(), Setpoint_Back, SetpointError_Rotation, GetPositionM2(), SetpointError_Translation);
            GetDirections();
            if (turnRight == true) {
                M1_Speed = 0.1; //turn to the right
                M1_Rotate = 1;
            } else if (turnLeft == true) {
                M1_Speed = 0.1; //turn to the left
                M1_Rotate = 0;
            } else if (turn == 1) {
                BurgerflipActie();  //flip the burger
            } else if (turn == 0) {
                M2_Speed = 0;   //do not flip the burger
                M3_Speed = 0;
            }
            if ((turnLeft == false) && (turnRight == false)) {
                M1_Speed = 0;      //do nothing

            }

        }
    }
}