File content as of revision 46:4af2f01da9f3:

#include "mbed.h"
#include <math.h>
#include "MODSERIAL.h"
#include "QEI.h"
#include "HIDScope.h"
#include "BiQuad.h"
 
/*
THINGS TO CONSIDER
- Motor action of motor 1 is inverted because it is mounted
opposite to motor 2 in the tower. Check if the clockwise directions of the 
motors correspond to the positive q1, q2-directions (both counterclockwise)
 in the original IK-sketch.
- Robot arms should be placed manually into reference position before resetting board.
*/
 
//set pins
DigitalIn encoder1A (D13);                          //Channel A from Encoder 1
DigitalIn encoder1B (D12);                              //Channel B from Encoder 1
DigitalIn encoder2A (D11);                                  //Channel A from Encoder 2
DigitalIn encoder2B (D10);                                      //Channel B from Encoder 2
AnalogIn emg0( A0 );
AnalogIn emg1( A1 );
DigitalIn button1(D3);
DigitalIn button2(D9);
 
DigitalOut motor1DirectionPin(D7);
PwmOut motor1MagnitudePin(D6);
DigitalOut motor2DirectionPin(D4);
PwmOut motor2MagnitudePin(D5);
DigitalOut ledGrn(LED_GREEN);
DigitalOut ledRed(LED_RED);
DigitalOut ledBlue(LED_BLUE);
 
//library settings
Serial pc(USBTX,USBRX);
HIDScope    scope(6);
 
//go-Ticker settings
Ticker MeasureTicker;
volatile bool MeasureTicker_go=false;
void MeasureTicker_act(){MeasureTicker_go=true;};               // Activates go-flag
 
//constant values
const float PI = 3.141592653589793;
const int cw = 0;               //values for cw and ccw are inverted! cw=0 and ccw=1
const int ccw = 1;
double threshold_l=0.09;                                        //left arm EMG threshold
double threshold_r = 0.08;                                      //right arm EMG threshold
float EMGgain = 1.0;
 
//set lengths
//float L0 = 0.232;                     //height of motor axes above table surface
float L1 = 0.45;                        //length of proximal arm
float L2 = 0.35;                            //length of distal arm
float TowerHeight = 0.232;                  //height of motor axes above table surface
float StampHeight = 0.056;              // height of end effector
float y_stampup = 0.1;                  //height stamp while not stamping: 10cm above                           table surface
float y_stampdown = -0.04;                  //height stamp while    stamping: at table surface
 
float MotorGain = 8.4;          // rad/s for PWM, is max motor speed
float MotorMaxSpeed = 0.1;                  //define a maximum PWM speed for the motor
float t_sample = 0.002;                                     //seconds
const float maxStampDistance = 0.7;
const float minStampDistance = 0.3;
float Kp = 4.0;                          //proportional controller constant
float Ki = 0.04;                            //integrative controller constant
float Kd = 0.02;                        //derivative controller constant
float N = 25;                               //PIDF filter constant
//(Higher N is faster derivative action but more sensitive to noise)
 
//set initial conditions for inputs and positions
float biceps_l = 0, biceps_r = 0;
double envelopeL = 0, envelopeR = 0;
int T=0;                                //EMG 'switch' variable
float ReferencePosition_x = 0.35;       //starting position for x reference position        
float ReferencePosition_y = L1 + TowerHeight - StampHeight; 
//starting position for y reference position
float ReferencePosition_xnew = 0.35;     
float ReferencePosition_ynew = L1 + TowerHeight - StampHeight;
float Position_x = 0.0;
float Position_y = 0.0;
 
//set initial conditions for angles, errors and motor values
float q1 = 0,q2 = PI/2;
float q1_ref = 0, q2_ref = 0;
float q1start = 0; 
float q12start = PI/2;
float q1Encoder = 0, q12Encoder = 0;
float q12Out = 0;
 
float q1_error_prev = 0, q2_error_prev = 0;
float DerTotalError1 = 0, DerTotalError2 = 0;
float q1IntError = 0, q2IntError = 0;
float TotalError1_prev = 0, TotalError2_prev = 0;
float TotalError1= 0, TotalError2= 0;
 
float motorValue1 = 0.0, motorValue2 = 0.0;
int counts1 = 0, counts2 = 0;
int counts1Prev = 0, counts2Prev = 0;
 
//set reference angle boundaries
float q1_refOutNew = 0, q2_refOutNew = 0;
float q1_refOutMin = 0;                 //Physical min angle 0.00 radians
float q1_refOutMax = 1.37;                  //Physical max angle 1.47 radians - 0.1 rad
float q2_refOutMin = 0.91;              //Physical  min angle 0.81 radians + 0.1 rad
float q2_refOutMax = 2.07;              //Physical max angle 2.17 radians - 0.1 rad
 
//set BiQuad filters/filter chains
BiQuad pidf;                    //PID Filter
 
BiQuadChain bcq1R;              //Right EMG filter chain 1: notch filter+highpass
BiQuadChain bcq2R;                  //Right EMG filter chain 2: lowpass
BiQuad bq1R(9.9110e-01,-1.6036e+00,9.9110e-01,-1.6036e+00,9.8221e-01);      
// Notch filter wo=50; bandwidth=wo/35
BiQuad bq2R(9.1497e-01,-1.8299e+00,9.1497e-01,-1.8227e+00,8.3718e-01);      
// High pass Butterworth filter 2nd order, Fc=10;
BiQuad bq3R(1.3487e-03,2.6974e-03,1.3487e-03,-1.8935e+00,8.9886e-01);       
// Low pass Butterworth filter 2nd order, Fc = 8;
 
BiQuadChain bcq1L;              //Left EMG filter chain 1: notch filter+highpass
BiQuadChain bcq2L;                  //Left EMG filter chain 2: lowpass
BiQuad bq1L(9.9110e-01,-1.6036e+00,9.9110e-01,-1.6036e+00,9.8221e-01);      
// Notch filter wo=50; bandwidth=wo/35
BiQuad bq2L(9.1497e-01,-1.8299e+00,9.1497e-01,-1.8227e+00,8.3718e-01);      
// High pass Butterworth filter 2nd order, Fc=10;
BiQuad bq3L(1.3487e-03,2.6974e-03,1.3487e-03,-1.8935e+00,8.9886e-01);       
// Low pass Butterworth filter 2nd order, Fc = 8;
// In the following: R is used for right arm EMG, L is used for left arm EMG
 
//define encoder counts and degrees
QEI Encoder1(D12, D13, NC, 32);                     // turns on encoder
QEI Encoder2(D10, D11, NC, 32);                         // turns on encoder
const int counts_per_revolution = 4200;                 //counts per motor axis revolution
const int inverse_gear_ratio = 131;
const float resolution = counts_per_revolution/(2*PI/inverse_gear_ratio);   //87567.0496892 counts per radian, encoder axis
 
//---------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------
 
void FilteredSample(int &Tout, double &envelopeLout, double &envelopeRout)
{   
//This function reads EMG, filters it and generates a T-switch value which specifies the movement of the robot
 
    double inLout = emg0.read();
    double inRout = emg1.read();
    
    double outRfilter1 = bcq1R.step(inRout);
    double outRrect= fabs(outRfilter1);
    envelopeRout = bcq2R.step(outRrect);
    
    double outLfilter1 = bcq1L.step(inLout);
    double outLrect = fabs(outLfilter1);
    envelopeLout = bcq2L.step(outLrect);
  
    double biceps_l = (double) envelopeLout * EMGgain; //emg0.read();
//left biceps filtered EMG signal with a gain

    double biceps_r = (double) envelopeRout * EMGgain;  
//right biceps filtered EMG signal with a gain
    if (biceps_l > threshold_l && biceps_r > threshold_r){
        //both arms activated: stamp moves down
        Tout = -2;
        }
    else if (biceps_l > threshold_l && biceps_r <= threshold_r){
        //arm 1 activated, move left
        Tout  = -1;
        }
    else if (biceps_l <= threshold_l && biceps_r > threshold_r){
        //arm 1 activated, move right
        Tout = 1;
        }
    else{
        //wait(0.2);
        Tout = 0;
        }   
}
 
//---------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------
 
void GetReferenceKinematics1(float &q1Out, float &q2Out, float &q1_refOut, float &q2_refOut)
{    
//This function reads current position from encoder, calculates reference position from 
//T-switch value, converts reference position to reference angles and checks boundaries.
    
    //get joint positions q feedback from encoder
    float Encoder1Position = counts1/resolution;    //angular encoder position (rad)
    float Encoder2Position = -1*counts2/resolution; //NEGATIVE due to opposite build                                    //up in tower
    
    q1Encoder = Encoder1Position*inverse_gear_ratio;
    q12Encoder = Encoder2Position*inverse_gear_ratio;
    q1Out = q1start + q1Encoder;                    //angular motor axis position (rad)
    q12Out = q12start + q12Encoder;                 //encoder 2 gives sum of q1 and q2!
    q2Out = q12Out - q1Out;             
    //float q1deg = q1Out*360/2/PI;                 //angle in degrees
    //float q2deg = q2Out*360/2/PI;                 //angle in degrees
    
    //get end effector position with trigonometry
    Position_x = (L1*sin(q1) + L2*sin(q1+q2));
    Position_y = (L1*cos(q1) + L2*cos(q1+q2)) + TowerHeight - StampHeight;  
    
    //get reference position from EMG based T-switch values  
    if (T == -2){
        //both EMGs active: stamp moves down
        ReferencePosition_xnew = ReferencePosition_x;
        ReferencePosition_ynew = ReferencePosition_y - 0.015;
        }
    else if (T==-1){
        //left EMG active, move stamp left
        ReferencePosition_xnew = ReferencePosition_x - 0.0009;
        ReferencePosition_ynew = y_stampup;
        }
    else if (T==1){
        //right EMG active, move stamp right
        ReferencePosition_xnew = ReferencePosition_x + 0.0009;
        ReferencePosition_ynew = y_stampup;
        }
    else{ //T==0
        //no EMG active, no x-movement, y-position restored to non-stamping position
        ReferencePosition_ynew = y_stampup;
        }
    
    //check reference position boundaries
    if (ReferencePosition_xnew > maxStampDistance){
        //target too far to be reached
        ReferencePosition_x = maxStampDistance;
        ReferencePosition_y = y_stampup;
        //pc.printf("Target too far! \r\n");
        }
    else if (ReferencePosition_xnew < minStampDistance){
        //target too close
        ReferencePosition_x = minStampDistance;
        ReferencePosition_y = y_stampup;
        //pc.printf("Target too close! \r\n");
        }
    else if (ReferencePosition_ynew < y_stampdown){
        //target under table surface
        ReferencePosition_x = ReferencePosition_xnew;
        ReferencePosition_y = y_stampdown;
        //pc.printf("Target too low! \r\n");
        }
    else {
        //target within reach
        ReferencePosition_x = ReferencePosition_xnew;        
        ReferencePosition_y = ReferencePosition_ynew;
        }
    
    //calculate reference joint angles for the new reference position
    float PointPositionArm2_x = ReferencePosition_x;
    float PointPositionArm2_y = ReferencePosition_y - TowerHeight + StampHeight;
    float PointVectorArm2 = sqrt(pow(PointPositionArm2_x,2)+pow(PointPositionArm2_y,2));
    
    float alpha = atan(PointPositionArm2_y/PointPositionArm2_x);
    float beta = acos((L2*L2-L1*L1-pow(PointVectorArm2,2))/(-2*L1*PointVectorArm2));
    q1_refOutNew = PI/2 - (alpha+beta);
    q2_refOutNew = PI - asin(PointVectorArm2*sin(beta)/L2);
    
    //check reference angle boundaries
    if (q1_refOutNew < q1_refOutMin){
        //new q1_ref too small
        q1_refOut = q1_refOutMin;
        //pc.printf("\r\n Under q1 angle boundaries\r\n");
        }
    else if (q1_refOutNew > q1_refOutMax){
        //new q1_ref too large
        q1_refOut = q1_refOutMax;
        //pc.printf("\r\n Above q1 angle boundaries\r\n");
        }
    else {
        //new q1_ref within reach
        q1_refOut = q1_refOutNew;
        }
        
    if (q2_refOutNew < q2_refOutMin){
        //new q2_ref too small
        q2_refOut = q2_refOutMin;
        pc.printf("\r\n Under q2 angle boundaries");
        }
    else if (q2_refOutNew > q2_refOutMax){
        //new q2_ref too large
        q2_refOut = q2_refOutMax;
        pc.printf("\r\n Above q2 angle boundaries");
        }
    else {
        //new q2_ref within reach
        q2_refOut = q2_refOutNew;
        }        
    
    scope.set(0, ReferencePosition_xnew);
    scope.set(1, ReferencePosition_ynew);
    scope.set(2, envelopeL);
    scope.set(3, envelopeR);
    scope.set(4, T);
    scope.send();
}    
 
//---------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------
 
void FeedbackControl1(float q1_ref, float q2_ref, float q1, float q2, float &motorValue1Out, float &motorValue2Out)
{
//This function calculates the error between angles and refernce angles, and provides
//motor values via a PIDF controller.
    
    //calculate error values
    float q1_error = q1_ref - q1;               // proportional angular error in radians
    float q2_error = q2_ref - q2;               // proportional angular error in radians
    
    //PIDF total error output
    float TotalError1 = pidf.step(q1_error);
    float TotalError2 = pidf.step(q2_error);
    
    //calculate motor values from the total errors    
    motorValue1Out = TotalError1/MotorGain;       
    motorValue2Out = TotalError2/MotorGain;       
    
    //prepare for next ticker cycle
    q1_error_prev = q1_error;
    q2_error_prev = q2_error;
    TotalError1_prev = TotalError1;
    TotalError2_prev = TotalError2;
}
 
//---------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------
 
void SetMotor1(float motorValue1, float motorValue2)
{
//This function sets the PWM and direction bits for the motors. 
//motorValues outside range are truncated to within range.
 
    //control motor 1
    if (motorValue1 >=0){ 
        //clockwise rotation
        motor1DirectionPin=cw;
        }
    else{
        //counterclockwise rotation 
        motor1DirectionPin=ccw;
        }
    if (fabs(motorValue1)>MotorMaxSpeed){
        motor1MagnitudePin = MotorMaxSpeed;             //motor values truncated
        }
    else{
        motor1MagnitudePin = fabs(motorValue1);
        }
    
    //control motor 2
    if (motorValue2 >=0){  
        //counterclockwise rotation due to inverse buildup in tower
        motor2DirectionPin=cw;                  //action is ccw, due to faulty                                  motor2DirectionPin (inverted)
        }
    else{    
        //clockwise rotation due to inverse buildup in tower
        motor2DirectionPin=ccw;                         //action is cw, due to faulty                                   motor2DirectionPin (inverted)
        }
    if (fabs(motorValue2)>MotorMaxSpeed){
        motor2MagnitudePin = MotorMaxSpeed;
        }
    else{
        motor2MagnitudePin = fabs(motorValue2);
        }
}
 
//---------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------
 
void MeasureAndControl()
{
// This function measures the EMG of both arms, calculates via inverse kinematics
// what the joint reference positions should be, and controls the motor with 
// a Feedback controller. This is called from a Ticker.
    FilteredSample(T, envelopeL, envelopeR);
    GetReferenceKinematics1(q1, q2, q1_ref, q2_ref);
    FeedbackControl1( q1_ref, q2_ref, q1, q2, motorValue1, motorValue2);
    SetMotor1(motorValue1, motorValue2);
}
 
//---------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------
 
int main()
{
 //pc.baud(115200);
 //pc.printf("Test putty IK");
 ledRed=1;
 ledBlue=1; 
 ledRed=0;

 //set BiQuad chains
 bcq1R.add(&bq1R).add(&bq2R);                       
 bcq2R.add(&bq3R);
 bcq1L.add(&bq1L).add(&bq2L);
 bcq2L.add(&bq3L);
 pidf.PIDF( Kp, Ki, Kd, N, t_sample );                  //set PID filter
 
 //start up encoders
 counts1 = Encoder1.getPulses();                        //gives position of encoder in counts 
 counts2 = Encoder2.getPulses();                        //gives position of encoder in counts
 wait(20.0);                                            //time to sart up HIDScope and EMG
 MeasureTicker.attach(&MeasureTicker_act, 0.002);       //initialize MeasureTicker, 500 Hz
 
 while(1)
    {
        if (MeasureTicker_go){
            MeasureTicker_go=false;
            ledGrn = 1;
            ledBlue = 0;
            MeasureAndControl();                    //execute MeasureAndControl
            counts1 = Encoder1.getPulses();         //get encoder counts again
            counts2 = Encoder2.getPulses();             //get encoder counts again
            ledBlue = 1;
            ledGrn = 0;
            }
    }
}