File content as of revision 18:cbe3fe6d89a7:

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

//======== 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 
AnalogIn emgr(A0); //should obviously be connected to the shield with an L label

// ticker
Ticker tick_part_cart;                  //ticker to switch to cart part 
Ticker tick_part_arm;                   //ticker to switch to arm part
Ticker tick_part_claw;                  //ticker to switch to claw part
Ticker sampleTicker;                    //ticker to sample the EMG signal
Ticker measureTicker;                   //ticker to read out encoder

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

//======== Variables ===========================================================
// 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;
int position_claw;
float ain_cart;     //Variable to store the analog input of the cart
float ain_arm;      //Variable to store the analog input of the arm

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

//P Controller
const double Setpoint_Cart = 0;
const double Setpoint_Arm = 0;
double Arm_ControlSpeed = 0;
double Cart_ControlSpeed = 0;
double SetpointError_Cart = 0;
double SetpointError_Arm = 0;
double setpoint;
//Cart 
const double Ts = 0.002; //Ts=1/fs (sample frequency)
const double Cart_Kp = 0.1, Cart_Ki = 0.0, Cart_Kd = 0.0;
double Cart_error = 0;
double Cart_e_prev = 0;
//Arm 
const double Arm_Kp = 0.1, Arm_Ki = 0.0, Arm_Kd = 0.0;
double Arm_error = 0;
double Arm_e_prev = 0;


// 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 cart
volatile int servo_id = 1;       //ID to the side the servo moves, begins in center
int waiting_claw = 1000;         //Servo delay to make claw control easier                

//======== Variables Filter ====================================================
/*coefficients of each filter
    lno  = left tricep notch filter at 50 Hz
    lno2 = left tricep notch filter at 100 Hz
    lhf  = left tricep high pass filter
    llf  = left tricep lowpass filter
    same goes for rno etc.
    */
double lno_b0 = 0.9911;     double rno_b0 = 0.9911;     
double lno_b1 = -1.6036;    double rno_b1 = -1.6036;  
double lno_b2 = 0.9911;     double rno_b2 = 0.9911; 
double lno_a1 = -1.603;     double rno_a1 = -1.603; 
double lno_a2 = 0.9822;     double rno_a2 = 0.9822;

double lno2_b0 = 0.9824;    double rno2_b0 = 0.9824;
double lno2_b1 = -0.6071;   double rno2_b1 = -0.6071;
double lno2_b2 = 0.9824;    double rno2_b2 = 0.9824;
double lno2_a1 = -0.6071;   double rno2_a1 = -0.6071;
double lno2_a2 = 0.9647;    double rno2_a2 = 0.9647;
   
double lhf_b0 = 0.9355;     double rhf_b0 = 0.9355;    
double lhf_b1 = -1.8711;    double rhf_b1 = -1.8711;  
double lhf_b2 = 0.9355;     double rhf_b2 = 0.9355;    
double lhf_a1 = -1.8669;    double rhf_a1 = -1.8669;   
double lhf_a2 = 0.8752;     double rhf_a2 = 0.8752;  

double llf_b0 = 8.7656e-5;  double rlf_b0 = 8.7656e-5;
double llf_b1 = 1.17531e-4; double rlf_b1 = 1.17531e-4;
double llf_b2 = 8.7656e-5;  double rlf_b2 = 8.7656e-5; 
double llf_a1 = -1.9733;    double rlf_a1 = -1.9733;  
double llf_a2 = 0.9737;     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 rno_y;
double lno2_y;  double rno2_y;
double lhf_y;   double rhf_y;
double llf_y;   double rlf_y;
double lrect_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
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 4 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;
}

//calibration to determine maximum EMG value
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;
}

//======== P Controller ========================================================
double p_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 4 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 signals (rlf_y and llf_y) are shown in channel 0 and 1 (scope.set)*/
void scopeSend(){
    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();
    }

void Measure(){
     // encoder Cart
     position_cart = (Encoder_Cart.getPulses()*factor_cart);    
     ain_cart = pot_cart.read();
     
     // encoder Arm
     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 (rlf_y > threshold_value_cart){
                if(position_cart <= -170){ 
                Cart.stop(1)==1; //If the cart is at the right side, it stops
                                
                }else if(position_cart >= 170 && position_arm <=-100){       
                Cart.stop(1) == 1; //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.
                
                }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){                                   
                Cart.stop(1)==1; ////If the cart is at the left side, it stops

                }else if(position_cart <= -170 && position_arm >=100){
                Cart.stop(1) == 1; //similar with the left side
                
                }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;
                }}
//Controller             
SetpointError_Cart =  setpoint - position_cart;
//set direction
    if (SetpointError_Cart < 0) {
        cart_speed = 0;
    }else{}
      
    Cart_ControlSpeed = Ts * fabs( p_control(SetpointError_Cart, Cart_Kp, Cart_Ki, Cart_Kd, Cart_error, Cart_e_prev));
    if (fabs(SetpointError_Cart) < fabs(Setpoint_Cart*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 (rlf_y > threshold_value_claw) {
                if(position_cart > -170 && position_arm >= 90){ //If the cart is not at            
                Arm.stop(1)==1; //the end, the arm can't move any further than 45 degrees 
                
                }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 
                Arm.stop(1)==1; //the right end, the arm can't move any further than 70 degrees
                
                }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){            
                Arm.stop(1)==1; //similar with the right side 
                
                }else if(position_cart < 170 && position_arm <= -60 && position_claw == -18){
                Arm.stop(1)==1;
                
                }else if(position_cart>=170 && position_arm<=-160){        
                Arm.stop(1)==1; //similar with the right side
                
                }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;
            }}
//Controller
SetpointError_Arm =  setpoint - position_arm;
//set direction
    if (SetpointError_Arm > 0) {
        arm_speed = 0;
    }else {}
    
    Arm_ControlSpeed = Ts * fabs( p_control(SetpointError_Arm, Arm_Kp, Arm_Ki, Arm_Kd, Arm_error, Arm_e_prev));
    if (fabs(SetpointError_Arm) < fabs(Setpoint_Arm*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;
                    led_b = not LedOn;
                    led_g = not LedOn;
                    servo.position(-18);
                    break;
                }
                case 1: {
                    led_b = LedOn;
                    led_r = not LedOn;
                    led_g = not LedOn;
                    servo.position(3);
                    wait_ms(waiting_claw);
                    break;
                }
                case 2: {
                    led_g = LedOn;
                    led_r = not LedOn;
                    led_b = not LedOn;
                    servo.position(27);
                    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);
                        break;
                    }
                    case 1: {
                        led_b = LedOn;                        
                        led_r = not LedOn;
                        led_g = not LedOn;
                        servo.position(3);
                        wait_ms(waiting_claw);
                        break;
                    }
                    case 2: {
                        led_g = LedOn;                        
                        led_r = not LedOn;
                        led_b = not LedOn;
                        servo.position(27);
                        break;
                    }
                }
                }else{}       
                }
            led_r = not LedOn;
            led_g = not LedOn;                
            position_claw = servo.read();
            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_cart.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);    
}