File content as of revision 17:18e9df406502:

#include "mbed.h"
#include "MODSERIAL.h"
#define SERIAL_BAUD 115200
#include "motordriver.h"
#include "QEI.h"
#include "Servo.h"
#include "HIDScope.h"

//======== Serial Communication ================================================
MODSERIAL pc(USBTX,USBRX);

//======== Motor and QEI =======================================================
int Brakeable;
int sign;

// motor
Motor Cart(D5, D4, D4, Brakeable);      // right motor
Motor Arm(D6,D7, D7, Brakeable);        // left motor

// qei
QEI Encoder_Cart(D10, D11, NC, 6400);
QEI Encoder_Arm(D12, D13, NC, 6400);

// servo
Servo servo(D9);

HIDScope    scope(2); //scope has two ports for the two EMG signals

//======== Miscellaneous =======================================================
// button
InterruptIn btn(SW2);
InterruptIn btn2(SW3);

InterruptIn btn_cart(D1);
InterruptIn btn_arm(D2);
InterruptIn btn_claw(D3);

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

// potmeter
AnalogIn pot_cart(A2);                  
AnalogIn pot_arm(A3);  

AnalogIn emgl(A1);                      // labels are attached to the olimex shields the left tricep should obviously be connected to the should with an L label on it
AnalogIn emgr(A0);

// ticker
Ticker tick_part;                       // ticker to switch parts 
Ticker tick_part_arm;  
Ticker tick_part_claw;  
Ticker sampleTicker; 
Ticker measureTicker; 
Ticker sampleTicker_claw; 

//======== Variables ===========================================================
// counters
int num_turned_on_0 = 0;                // count number of times red LED turned on
int num_turned_on_1 = 0;                // count number of times green LED turned on
int num_turned_on_2 = 0;                // count number of times blue LED turned on

int num_claw_turned_on_0 = 0;           // count number of times red LED turned on
int num_claw_turned_on_1 = 0;           // count number of times green LED turned on
int num_claw_turned_on_2 = 0;           // count number of times blue LED turned on

// speed
double cart_speed = 0.2;
double arm_speed = 0.1;

// position
float factor_cart = 0.05802;
float factor_arm = 0.1539;
int position_cart;
int position_arm;
float ain_cart;     //Variable to store the analog input of the cart
float ain_arm;      //Variable to store the analog input of the arm

int position_claw;


// miscellaneous
const float kTimeToggle = 0.05f;        // period with which to toggle the parts
const int LedOn = 0;                    // LED on if 0
volatile int part_id = 1;               // ID of what part should move, begins with the cart
volatile int servo_id = 1;              // ID to the side the servo should move, begins in center position

int waiting_claw = 1000;

//======== Variables Filter ====================================================
/*coefficients of each filter
    lno = left tricep notch filter
    lhf = left tricep high pass filter
    llf = left tricep lowpass filter
    same goes for rno etc.
    */
double lno_b0 = 0.9911;     
double lno_b1 = -1.6036;   
double lno_b2 = 0.9911;    
double lno_a1 = -1.603;    
double lno_a2 = 0.9822;    

double rno_b0 = 0.9911;     
double rno_b1 = -1.6036;   
double rno_b2 = 0.9911;    
double rno_a1 = -1.603;    
double rno_a2 = 0.9822;    

double lno2_b0 = 0.9824;
double lno2_b1 = -0.6071;
double lno2_b2 = 0.9824;
double lno2_a1 = -0.6071;
double lno2_a2 = 0.9647;
   
double rno2_b0 = 0.9824;
double rno2_b1 = -0.6071;
double rno2_b2 = 0.9824;
double rno2_a1 = -0.6071;
double rno2_a2 = 0.9647;

double lhf_b0 = 0.9355;    
double lhf_b1 = -1.8711;   
double lhf_b2 = 0.9355;    
double lhf_a1 = -1.8669;   
double lhf_a2 = 0.8752;   

double rhf_b0 = 0.9355;    
double rhf_b1 = -1.8711;   
double rhf_b2 = 0.9355;    
double rhf_a1 = -1.8669;   
double rhf_a2 = 0.8752;    


double llf_b0 = 8.7656e-5;
double llf_b1 = 1.17531e-4;
double llf_b2 = 8.7656e-5; 
double llf_a1 = -1.9733;  
double llf_a2 = 0.9737;    

double rlf_b0 = 8.7656e-5;
double rlf_b1 = 1.17531e-4;
double rlf_b2 = 8.7656e-5; 
double rlf_a1 = -1.9733;  
double rlf_a2 = 0.9737;    


//starting values of the biquads of the corresponding filters
double lno_v1 = 0, lno_v2 = 0;
double lno2_v1 = 0, lno2_v2 = 0;
double lhf_v1 = 0, lhf_v2 = 0;
double llf_v1 = 0, llf_v2 = 0;

double rno_v1 = 0, rno_v2 = 0;
double rno2_v1 = 0, rno2_v2 = 0;
double rhf_v1 = 0, rhf_v2 = 0;
double rlf_v1 = 0, rlf_v2 = 0;

/* declaration of the outputs of each biquad.
the output of the previous biquad is the input for the next biquad.
so lno_y goes into lhf_y etc.
*/ 
double lno_y;
double lno2_y;
double lhf_y;
double llf_y;
double lrect_y;
double rno_y;
double rno2_y;
double rhf_y;
double rlf_y;
double rrect_y;


// set the threshold value for the filtered signal
//if the signal exceeds this value the motors will start to rotate 
const double threshold_value= 0.1;
const double threshold_value_claw= 0.08;
const double threshold_value_cart=0.08;



/* declaration of each biquad
The coefficients will be filled in later on in void scopeSend
As said before the input of each biquad is the output of the previous one 
The input of the first biquad is the raw EMG signal and the output of the last biquad is the filtered signal.
This is done for both left and right so this makes two chains of 3 biquads */

double biquad_lno(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;
}

double biquad_lno2(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;
}

double biquad_lhf(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;
}

double biquad_llf(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;
}
double biquad_rno(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;
}

double biquad_rno2(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;
}

double biquad_rhf(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;
}

double biquad_rlf(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;
}

int t = 0;
double value_right = 0;
double value_left = 0;

double calibrate_right(){
    for(t=0; t<200; t++){
      if(value_right <= rlf_y){
          value_right = rlf_y; 
        }else{
        value_right = value_right +0.0 ; 
        }
        }
        return value_right;
}

double calibrate_left(){
    for(t=0; t<200; t++){
      if(value_left <= llf_y){
          value_left = llf_y; 
        }else{
        value_left = value_left +0.0 ; 
        }
        }
        return value_left;
}


/* function that calculates the filtered EMG signal from the raw EMG signal. 
So 2 chains of 3 biquads each are calculating the left and the right filtered EMG signal.
After this is calculated, the signals are sent to HIDscope (scope.send) to see what they look like.
The filtered left signal (llf_y) is shown in channel 1, the filtered right signal (rlf_y)is shown in channel 0 (scope.set)*/

//==========PID================
//setpoints
const double Setpoint_Translation = -200;
const double Setpoint_Back = 0;
const double Setpoint_Rotation = 0;
double M3_ControlSpeed = 0;
double M2_ControlSpeed = 0;
double SetpointError_Translation = 0;
double SetpointError_Rotation = 0;

//booleans for control
bool booltranslate = false;
bool boolrotate = false;


    double setpoint;
    double Arm_ControlSpeed = 0;
    double Cart_ControlSpeed = 0;

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

//Arm PID
const double Rotation_Kp = 0.0499, Rotation_Ki = 0.0429 , Rotation_Kd = 0.0429;
double Rotation_error = 0;
double Rotation_e_prev = 0;

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



//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
//======== Functions and main ==============================================================
/* function that calculates the filtered EMG signal from the raw EMG signal. 
So 2 chains of 3 biquads each are calculating the left and the right filtered EMG signal.
After this is calculated, the signals are sent to HIDscope (scope.send) to see what they look like.
The filtered left signal (llf_y) is shown in channel 1, the filtered right signal (rlf_y)is shown in channel 0 (scope.set)*/
void scopeSend(void){
    lno_y = biquad_lno(emgl.read(), lno_v1, lno_v2, lno_a1, lno_a2, lno_b0, lno_b1, lno_b2);
    lno2_y = biquad_lno2(lno_y, lno2_v1, lno2_v2, lno2_a1, lno2_a2, lno2_b0, lno2_b1, lno2_b2);
    lhf_y = biquad_lhf(lno2_y, lhf_v1, lhf_v2, lhf_a1, lhf_a2, lhf_b0, lhf_b1, lhf_b2);
    lrect_y = fabs(lhf_y);
    llf_y = biquad_llf(lrect_y, llf_v1, llf_v2, llf_a1, llf_a2, llf_b0, llf_b1, llf_b2)/value_left;
    
    rno_y = biquad_rno(emgr.read(), rno_v1, rno_v2, rno_a1, rno_a2, rno_b0, rno_b1, rno_b2);
    rno2_y = biquad_rno2(rno_y, rno2_v1, rno2_v2, rno2_a1, rno2_a2, rno2_b0, rno2_b1, rno2_b2);    
    rhf_y = biquad_rhf(rno2_y, rhf_v1, rhf_v2, rhf_a1, rhf_a2, rhf_b0, rhf_b1, rhf_b2);
    rrect_y = fabs(rhf_y);
    rlf_y = biquad_rlf(rrect_y, rlf_v1, rlf_v2, rlf_a1, rlf_a2, rlf_b0, rlf_b1, rlf_b2)/value_right;
    
    scope.set(1, llf_y);
    scope.set(0, rlf_y);
    scope.send();
    }

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
//======== Functions and main ==============================================================
void Measure(){
     // encoder
     position_cart = (Encoder_Cart.getPulses()*factor_cart) ;    
     ain_cart = pot_cart.read();
     
     // encoder
     position_arm = (Encoder_Arm.getPulses()*factor_arm) ;    
     ain_arm = pot_arm.read();
   
     if (ain_arm == 0){
        Encoder_Arm.reset();   
     }else {}
     
     
    
}    


// Switch between Cart, Arm and Claw

void SwitchCart(){
    switch (part_id) {
        //Cart
        case 2: {
            led_r = LedOn;         
            if(led_r == LedOn){
                num_turned_on_0++;
            
                if (rlf_y > threshold_value_cart) {
                    if(position_cart <= -170){                                  //If the cart is at the right side, it stops
                    Cart.stop(1)==1;
                                    
                    }else if(position_cart >= 170 && position_arm <=-100){       //If the cart is at the left side and the arm is rotated 60 degrees to the left, the cart can't move to the right.
                    Cart.stop(1) == 1;
                    
                    }else if(position_cart >= 170 && position_arm <=-160 && position_claw == -18){
                    Cart.stop(1) == 1;    
    
                    }else{
                    Cart.speed(cart_speed)==cart_speed;
                    }
                    
                        if (llf_y > threshold_value_cart){
                            Cart.stop(1)==1;
                        }
         
                }else if (llf_y > threshold_value_cart) {
                    if(position_cart >= 170){                                   //If the cart is at the left side, it stops
                    Cart.stop(1)==1;

                    }else if(position_cart <= -170 && position_arm >=100){       //If the cart is at the left side and the arm is rotated 60 degrees to the left, the cart can't move to the right.
                    Cart.stop(1) == 1;
                    
                    }else if(position_cart <= -170 && position_arm >=160 && position_claw == 27){
                    Cart.stop(1)==1;
                    
                    }else{
                    Cart.speed(-cart_speed)==-cart_speed;
                    }
                    
                        if (rlf_y > threshold_value_cart){
                            Cart.stop(1)==1;
                        }
                                            
                }else {
                    Cart.stop(1)==1;    
                }
            }
            
            if(!btn && !btn2) {
                    Arm.speed(0) == 0;
                    if(position_cart<0){
                    Cart.speed(-0.1)== -0.1;
                    }else if(position_cart>0){
                    Cart.speed(0.1)==0.1;
                    }else{
                    Cart.stop(0)==0;
                    }
             }
             
            SetpointError_Translation =  setpoint - position_cart;

    //set direction
    if (SetpointError_Translation < 0) {
        cart_speed = 0;
    } else {}
    
    
    Cart_ControlSpeed = Ts * fabs( pid_control(SetpointError_Translation, Translation_Kp, Translation_Ki, Translation_Kd, Translation_error, Translation_e_prev));
    if (fabs(SetpointError_Translation) < fabs(Setpoint_Translation*0.05)) {
        cart_speed = 0;
        
    }
      else{
    cart_speed = Cart_ControlSpeed;
    } 
            
              
            // controle LED    
            led_g = not LedOn;
            led_b = not LedOn;     

            pc.baud(115200);
            pc.printf("Distance in mm: %i\n", position_cart);
            
            break;
        }
      }
}

void SwitchArm(){
    switch (part_id) {
        //Cart
        case 3: {
            led_g = LedOn;         
            if(led_g == LedOn){
                num_turned_on_1++;
                Arm.stop(1)==1;

            
                if (rlf_y > threshold_value_claw) {
                    if(position_cart > -170 && position_arm >= 90){             //If the cart is not at the end, the arm can't move any further than 45 degrees
                    Arm.stop(1)==1;
                    
                    }else if(position_cart > -200 && position_arm >= 60 && position_claw == 27){
                    Arm.stop(1)==1;
                        
                    }else if(position_cart<= -170 && position_arm>=160){         //If the cart is at the right end, the arm can't move any further than 70 degrees
                    Arm.stop(1)==1;
                    
                    }else{
                    Arm.speed(arm_speed)==arm_speed;
                    } 
                    
                    if (llf_y > threshold_value){
                            Cart.stop(1)==1;
                        }
         
                }else if (llf_y > threshold_value_claw) {
                    if(position_cart < 170 && position_arm <= -90){             //If the cart is not at the end, the arm can't move any further than 45 degrees 
                    Arm.stop(1)==1;
                    
                    }else if(position_cart < 170 && position_arm <= -60 && position_claw == -18){
                    Arm.stop(1)==1;
                    
                    }else if(position_cart>=170 && position_arm<=-160){          //If the cart is at the left end, the arm can't move any further than 70 degrees
                    Arm.stop(1)==1;
                    
                    }else{
                    Arm.speed(-arm_speed)==-arm_speed;
                    } 
                    
                        if (rlf_y > threshold_value){
                        Cart.stop(1)==1;
                        }
                        
                }else {
                    Arm.stop(1)==1;
                }
                
                if(!btn&&!btn2){
                    Cart.speed(0) == 0;
                if(position_arm>0){
                Arm.speed(-0.1)== -0.1;
                }else if(position_arm<0){
                Arm.speed(0.1)==0.1;
                }else{
                Arm.stop(0)==0;
                    }    
                    }
            } 
            
            {

    SetpointError_Rotation =  setpoint - position_arm;

    //set direction
    if (SetpointError_Rotation > 0) {
        arm_speed = 0;
    } else {}
    
    Arm_ControlSpeed = Ts * fabs( pid_control(SetpointError_Rotation, Rotation_Kp, Rotation_Ki, Rotation_Kd, Rotation_error, Rotation_e_prev));
    if (fabs(SetpointError_Rotation) < fabs(Setpoint_Rotation*0.05)) {
        Arm_ControlSpeed = 0;
    }else{
        arm_speed = Arm_ControlSpeed;
    }
}
            
            
            // controle LED   
            led_r = not LedOn;
            led_b = not LedOn;           
            
            pc.baud(115200);
            pc.printf("Degrees: %i\n", position_arm);
            
            break;
        }
      }
}

void SwitchClaw(){
switch (part_id) {
    case 4: {
    led_b = LedOn;

           if(rlf_y > threshold_value_claw){
           servo_id ++;
                 
                switch (servo_id) {
                    case 0: {
                        led_r = LedOn;
                        if (led_r == LedOn) {
                                num_claw_turned_on_0++;
                            }
                            
                            led_b = not LedOn;
                            led_g = not LedOn;
                            servo.position(-18);
                            //pc.printf("Servo position is: left \r\n");
                            
                            break;
                        }
                        case 1: {
                            led_b = LedOn;
                            if (led_b == LedOn) {
                                num_claw_turned_on_1++;
                            }
                            
                            led_r = not LedOn;
                            led_g = not LedOn;
                            
                            servo.position(3);
                            //pc.printf("Servo position is: center \r\n");
                            
                            wait_ms(waiting_claw);
                            
                            break;
                        }
                        case 2: {
                            led_g = LedOn;
                            if (led_g == LedOn) {
                                num_claw_turned_on_2++;
                            }
                            
                            led_r = not LedOn;
                            led_b = not LedOn;
                            
                            servo.position(27);
                            //pc.printf("Servo position is: %i\r\n", num_claw_turned_on);
                            
                            break;
                        }
                    }
                 
                 
                }else if(llf_y > threshold_value_claw){
                servo_id --;
                
                switch (servo_id) {
                    case 0: {
                        led_r = LedOn;                       
                        led_b = not LedOn;
                        led_g = not LedOn;
                        
                        servo.position(-18);
                        //pc.printf("Servo position is: left \r\n");

                        
                        break;
                    }
                    case 1: {
                        led_b = LedOn;                        
                        led_r = not LedOn;
                        led_g = not LedOn;
                        
                        servo.position(3);
                        //pc.printf("Servo position is: center \r\n");
                        
                        wait_ms(waiting_claw);
                        
                        break;
                    }
                    case 2: {
                        led_g = LedOn;                        
                        led_r = not LedOn;
                        led_b = not LedOn;
                        
                        servo.position(27);
                        //pc.printf("Servo position is: right \r\n");

                        
                        break;
                    }
                }
                
                }else{}       
            }
            led_r = not LedOn;
            led_g = not LedOn;                
            
            position_claw = servo.read();
            pc.baud(115200);
            pc.printf("Servo position is: %i, %i, %i \r\n", num_claw_turned_on_0, num_claw_turned_on_1, num_claw_turned_on_2);
            
            break;
        }    
}


// Switch the part
void SetValue2() {
    part_id = 2;
}

void SetValue3() {
    part_id = 3;
}


void SetValue4() {
    part_id = 4;
}


// Main
int main()
{
    led_r = not LedOn;
    led_g = not LedOn;
    led_b = not LedOn;
    
    tick_part.attach(&SwitchCart,kTimeToggle);
    tick_part_arm.attach(&SwitchArm,kTimeToggle);
    tick_part_claw.attach(&SwitchClaw,0.1f);   
    measureTicker.attach(Measure, 0.005);
    sampleTicker.attach(scopeSend,0.01); 
   
   
    btn_cart.fall(&SetValue2);
    btn_arm.fall(&SetValue3);
    btn_claw.fall(&SetValue4);

    if(part_id == 4){
        Cart.stop(1)==1;
        }

    while (true);
    
}