File content as of revision 7:75d2244d7745:

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

Serial pc (USBTX,USBRX);
//HIDScope scope(2);

PwmOut Motor1pwm(D6);            //motor 1 velocity control
DigitalOut Motor1Dir(D7);        //motor 1 direction
PwmOut Motor2Vel(D5);            //motor 2 velocity control
DigitalOut Motor2pwm(D4);        //motor 2 direction
DigitalOut led1(D2);
AnalogIn emg1(A0);
AnalogIn emg2(A1);
AnalogIn emg3(A2);
DigitalOut Button(D3);
DigitalOut ledb(LED_BLUE);
DigitalOut ledg(LED_GREEN);
DigitalOut ledr(LED_RED);

QEI encoder1(D10,D11,NC,16);      //define encoder pins
QEI encoder2(D8,D9,NC,16);

BiQuad bq1 ( 0.9150 , -1.8299 , 0.9150 , 1.0000 , -1.8227 , 0.8372); //Highpass
BiQuad bq2 ( 0.0001 ,   0.0002  ,  0.0001  ,  1.0000 ,  -1.9733  ,  0.9737); //Lowpass

Ticker motors_tick;
Ticker emg_tick;

float Angle1,Angle2;              //real angles of the motor
float XPos, YPos;                 //position of the end-effector
float XSet = 42, YSet = 0;                 //desired position of the end-effector
float L = 30.0;                  //length of the arms of the robot
const float Ts = 0.01;            //to control the tickers
const float KV = 1;               //velocity constant, to scale the desired velocity of the end-effector
const float P = 0.1;

const float pi = 3.14159265359,PulsesPerRev = 16.0,GearRatio = 131.25*3;   //to convert pulses to radians 
const float Angle1_0 = pi/2 ,Angle2_0 = 0;    //initial angle of the motors

//variables and constants for the PID
const float KP = 200, KI = 0.1, KD = 6, N = 100;
float M1_v1 = 0, M1_v2 = 0, M2_v1 = 0, M2_v2 = 0;

volatile double a = 0, b = 0, c = 0;      //variables to store the highest emg signal to normalize the data
double Y1, Y2;                // Y1 and Y2 are taken to conform Y to be used to change the set position X is also used to change the set position
volatile double Y, X;          

volatile bool ReturnHP = false;       //to keep calling HomePosition until getting to the setpoint
volatile bool calibration = true;     //to call FirstMovement the first loop, when the program is run
int counter = 0;                      //to count the time of inactivity

double emg1filt()
{
    double xtemp=(bq2.step(fabs(bq1.step(emg1))));
    if (xtemp>a)
        a=xtemp;
         
    X = xtemp/a;
    return X;
}

double emg2filt()
{
    double y1temp = (bq2.step(fabs(bq1.step(emg1))));
    if (y1temp>b)
        b=y1temp;
        
    Y1 = y1temp/b;
    return Y1;
}

int emg3filt()
{
    double y2temp = (bq2.step(fabs(bq1.step(emg1))));
    if (y2temp>c)
    {
        c=y2temp;
    }    
    Y2 = y2temp/c;
    if(Y2 < 0.3)
        Y2 = 1;
    else
        Y2 = -1;
    return Y2;
}

void callemgfilt()
{
    X = emg1filt();
    Y = emg2filt()/**emg3filt()*/;  // emg2filt gives the amplitude of Y and emg3filt the sign    
}

// PID controller (Tustin approximation)
float PID(float err, const float KP, const float KI, const float KD,const float Ts, const float N, float &v1, float &v2)
{
    const float a1 = -4/(N*Ts+2);
    const float a2 = -(N*Ts-2)/(N*Ts+2);
    const float b0 = (4*KP+4*KD*N+2*KI*Ts+2*KP*N*Ts+KI*N*pow(Ts,2))/(2*N*Ts+4);
    const float b1 = (KI*N*pow(Ts,2)-4*KP-4*KD*N)/(N*Ts+2);
    const float b2 = (4*KP+4*KD*N-2*KI*Ts-2*KP*N*Ts+2*KI*N*pow(Ts,2))/(2*N*Ts+4);
    
    float v = err-a1*v1-a2*v2;
    float u = abs(b0*v+b1*v1+b2*v2);
    v2 = v1; v1 = v;
    return u;
}

void positionxy(float &Angle1,float &Angle2,float &XPos,float &YPos)
{ 
    Angle1 = -(encoder1.getPulses()/(2*PulsesPerRev*GearRatio))*2*pi + Angle1_0;   //angles of the arms driven by the motors in radians
    Angle2 = -(encoder2.getPulses()/(2*PulsesPerRev*GearRatio))*2*pi + Angle2_0;
    
    XPos = L*cos(Angle1)+L*cos(Angle2);    // calculate the position of the end-effector
    YPos = L*sin(Angle1)+L*sin(Angle2); 
    //pc.printf("Angle1: %f Angle2: %f\r\n",Angle1,Angle2);
}

int Direction(float err)
{   //choose the direction of the motor, using the difference between the set angle and the actual angle.
    int a;
    if(err >= 0) 
        a = 1;
    else
        a = 0;
    return a;
}

void MoveMotors(float XSet, float YSet)    // move the motors using the inverse kinematic equations of the robot
{   
    float VX = KV*(XSet - XPos);     //set the desired velocity, proportional to the difference between the set point and the end-effector position.
    float VY = KV*(YSet-YPos);
    
    float W1 = (VX*(XPos-L*cos(Angle1))+VY*(YPos-L*sin(Angle1)))/(L*YPos*cos(Angle1)-L*XPos*sin(Angle1)); // calculate the needed angular velocity to achieve the desired velocity
    float W2 = (-VX*XPos-VY*YPos)/(L*YPos*cos(Angle1)-L*XPos*sin(Angle1));
    
    float Angle1Set = Angle1 + W1*Ts;   //calculate the set angle, angle at which the motor should be after 1 period with the calculated angular velocity
    float Angle2Set = Angle2 + W2*Ts;
    
    Motor1pwm = PID(Angle1Set-Angle1,KP,KI,KD,Ts,N,M1_v1,M1_v2)*P;;
    Motor1Dir = Direction(Angle1Set-Angle1);;
    Motor2Vel = PID(Angle2Set-Angle2,KP,KI,KD,Ts,N,M2_v1,M2_v2)*P;;
    Motor2pwm = !Direction(Angle2Set-Angle2);
    pc.printf("Angle1: %f Angle2: %f XPos: %f YPos: %f \r\n", Angle1, Angle2, XPos, YPos );
    
}

void FirstMovement()//function to get the end-effector to the workspace (with safety margin) to go from there to the home position
{
    ledr = 0;
    XSet = 30;
    YSet = 0;
    positionxy(Angle1,Angle2,XPos,YPos); 
    MoveMotors(XSet,YSet);
    
    if(pow(XSet-XPos,2)+pow(YSet-YPos,2) < pow(0.2,2))
    {//when the motor is close to the set angle(inside the safe workspace) go to the home position
        ledr = 1;
        calibration = false;
        ReturnHP = true;
    }
}

void GoToSet()
{   
    ledg = 0;
    ledb = 1;
    XSet = 42+X*10;  // keep increasing the set point, depending on the input
    //YSet += Y*0.1;
    //XSet = 46;
    YSet = 0;
    MoveMotors(XSet,YSet);  
      
}

void HomePosition()  //Go back to the initial position
{  
    ledb = 0;
    ledg = 1;
    const float  AllowedError = 0.5;  
    const float X0 = 25, X1 = 21.85, X2 = 31.94, slope = -17.34/6.6;  //values of special points in the workspace
    
    positionxy(Angle1,Angle2,XPos,YPos);
    if(sqrt(pow(XPos-XSet,2)+pow(YPos-YSet,2))<AllowedError)  //to call the function until the end effector is at the initial position
    {
        ReturnHP = false;
    }
    else
    {
        ReturnHP = true; 
    }
    //The following if-else statement is to choose a set point so that the end-effector does not go out of the work space while returning 
    if(XPos<X1)
    {
        XSet = X0;
        YSet = 0;
    }
    else if(XPos < X2 && (XPos < slope*YPos+X0 || XPos < slope*YPos+X0 ))
    {
        XSet = XPos;
        YSet = 0;
    }
    else if(XPos >= X2 && (XPos < slope*YPos+X0 || XPos < slope*YPos+X0))
    {
        XSet = X2;
        YSet = 0;
    }
    else
    {
        XSet = X0;
        YSet = 0;
    }
    MoveMotors(XSet,YSet);
}

void CallFuncMovement()   //decide which function has to be used
{   

    X = emg1.read(); 
    Y = emg2.read();                   
    positionxy(Angle1,Angle2,XPos,YPos);     //calculate the position of the end point
    if (Button == 1)
    {  //stop motors
        Motor1pwm = 0;
        Motor2pwm = 0;
    }
    else if(calibration)
    {
        FirstMovement();  
    }
    else if(ReturnHP)
    {   //when InitialPosition is called, keep calling until the end-effector gets there;
        HomePosition();
        counter = 0;
    }
    else if (sqrt(pow(XPos-300,2) + pow(YPos,2)) > 280 || sqrt(pow(XPos,2) + pow(YPos,2)) > 570 || sqrt(pow(XPos,2) + pow(YPos-300,2)) > 320 || sqrt(pow(XPos,2) + pow(YPos+300,2)) > 320 ) 
    { // limit so that the robot does not break. 
        HomePosition();
        counter = 0;    
    }
    else if (counter >= 200)
    {  //after 2 seconds of inactivity the robot goes back to the initial position
        HomePosition();
        counter = 0;
    }
    else if (X > 0 || Y != 0)
    {
        GoToSet(); 
        counter = 0; 
    }
    else 
    {
        counter++;
    } 
}

main()
{
    ledb = 1;
    ledg = 1;
    pc.baud(115200);
    //emg_tick.attach(&callemgfilt,0.001);
    motors_tick.attach(&CallFuncMovement,Ts);
    while (true)
    { /*keep the program running*/}
}