File content as of revision 41:68b170829965:

#include "mbed.h"
#include <math.h>
#include "MODSERIAL.h"
#include "QEI.h"
#include "HIDScope.h"
#include "BiQuad.h"

/*
THINGS TO CONSIDER
- Line 234, 239: 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.
- Line 244,257: motor values have been scaled down for safety at first test, restore
after testing to get proper action.
- Set angle and length boundaries!!
- Set robot constants (lengths etc.)
- Set EMGgain and thresholds
- Add tower height to ReferencePosition_y and Position_y AND inverse kinematics calculation!
- Add (lower) boundaries to TotalErrors
- MotorGain could change due to arm weight!!
- Arms should be placed manually into reference position.
*/

//set pins
DigitalIn encoder1A (D13); //Channel A van Encoder 1
DigitalIn encoder1B (D12); //Channel B van Encoder 1
DigitalIn encoder2A (D11); //Channel A van Encoder 2, kan niet op D15
DigitalIn encoder2B (D10); //Channel B van Encoder 2, kan niet op D14
//DigitalOut led1 (D11); 
//DigitalOut led2 (D10);
//AnalogIn potMeter1(A2);
//AnalogIn potMeter2(A1);
DigitalOut motor1DirectionPin(D7);
PwmOut motor1MagnitudePin(D6);
DigitalOut motor2DirectionPin(D4);
PwmOut motor2MagnitudePin(D5);
DigitalIn button1(D3);
DigitalIn button2(D9);
AnalogIn    emg0( A0 );
AnalogIn    emg1( A1 );

DigitalOut ledGrn(LED_GREEN);
DigitalOut ledRed(LED_RED);
DigitalOut ledBlue(LED_BLUE);

//library settings
Serial pc(USBTX,USBRX);
Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
HIDScope    scope(6);

//initial values
float dx;
float dy;
double DerivativeCounts;
//float referenceVelocity = 0;
//float bqcDerivativeCounts = 0;
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;

//set lengths (VALUES HAVE TO BE CHANGED)
//float x0 = 1.0;
float L0 = 0.232;
float L1 = 0.45;
float L2 = 0.35;
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


//set initial conditions
float biceps_l = 0;
float biceps_r = 0;
float ReferencePosition_x = 0.4;        //L2;
float ReferencePosition_y = L1 + TowerHeight - StampHeight;
float ReferencePosition_xnew = 0.4;     //L2;
float ReferencePosition_ynew = L1 + TowerHeight - StampHeight;
float Position_x = 0.0;
float Position_y = 0.0;
float q1 = 0;
float q2 = PI/2;
float q1_ref = 0;
float q2_ref = 0;
float q1start = 0;
float q12start = PI/2;
float q1Encoder = 0;
float q12Encoder = 0;
float q12Out = 0;

float q1_error_prev = 0;
float q2_error_prev = 0;
float DerTotalError1 = 0;
float DerTotalError2 = 0;
float q1IntError = 0;
float q2IntError = 0;
float TotalError1_prev = 0;
float TotalError2_prev = 0;

float motorValue1 = 0.0;
float motorValue2 = 0.0;
int counts1 = 0;
int counts2 = 0;
int counts1Prev = 0;
int counts2Prev = 0;
double envelopeL = 0;
double envelopeR = 0;

//set constant or variable values (VALUES HAVE TO BE EDITED)
int T=0;        //EMG 'switch' variable
double threshold_l=0.1;   //left arm EMG threshold
double threshold_r = 0.08; //right arm EMG threshold
float EMGgain = 1.0;

float dy_stampdown = 2.0; //0.05; //5 cm movement downward to stamp
float MotorGain = 8.4; // rad/s for PWM, is max motor speed (motor value of 1)
float Motor1ExtraGain = 1.0;
float MotorMaxSpeed = 0.1;  //define a maximum PWM speed for the motor
float t_sample = 0.01; //seconds
const float maxStampDistance = 0.7;       //0.66;
const float minStampDistance = 0.35;
float Kp = 4.0;//potMeter2.read();
float Ki = 0.02;  //0.01*Kp; //potMeter2.read();
float Kd = 0.02;     //0.1;(unstable!) //0.05; //0.02;        //0.04*Kp; //potMeter2.read();
float N = 25; //N=1/Tf, Higher N is faster derivative action but more sensitive to noise.

float q1_refOutNew = 0;
float q1_refOutMin = 0;             //Physical min angle 0.00 radians + 0.1 rad
float q1_refOutMax = 1.37;          //Physical max angle 1.47 radians - 0.1 rad
float q2_refOutNew = 0;
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
float TotalError1= 0;
float TotalError2= 0;
float TotalErrorMin= 0;
 
//set BiQuad
BiQuadChain bqc;
BiQuad bq1(0.0186,    0.0743,    0.1114,    0.0743,    0.0186); //get numbers from butter filter MATLAB
BiQuad bq2(1.0000,   -1.5704,    1.2756,   -0.4844,    0.0762);

BiQuad pidf;

BiQuadChain bcq1R;
BiQuadChain bcq2R;
// Notch filter wo=50; bw=wo/35
BiQuad bq1R(9.9110e-01,-1.6036e+00,9.9110e-01,-1.6036e+00,9.8221e-01);
// High pass Butterworth filter 2nd order, Fc=10;
BiQuad bq2R(9.1497e-01,-1.8299e+00,9.1497e-01,-1.8227e+00,8.3718e-01);
// Low pass Butterworth filter 2nd order, Fc = 8;
BiQuad bq3R(1.3487e-03,2.6974e-03,1.3487e-03,-1.8935e+00,8.9886e-01);

BiQuadChain bcq1L;
BiQuadChain bcq2L;
// Notch filter wo=50; bw=wo/35
BiQuad bq1L(9.9110e-01,-1.6036e+00,9.9110e-01,-1.6036e+00,9.8221e-01);
// High pass Butterworth filter 2nd order, Fc=10;
BiQuad bq2L(9.1497e-01,-1.8299e+00,9.1497e-01,-1.8227e+00,8.3718e-01);
// Low pass Butterworth filter 2nd order, Fc = 8;
BiQuad bq3L(1.3487e-03,2.6974e-03,1.3487e-03,-1.8935e+00,8.9886e-01);
// In the following: R is used for right arm, L is used for left arm!

//set go-Ticker settings
volatile bool MeasureTicker_go=false, BiQuadTicker_go=false, FeedbackTicker_go=false, TimeTracker_go=false; // sampleT_go=false;
void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags
void BiQuadTicker_act(){BiQuadTicker_go=true;};
void FeedbackTicker_act(){FeedbackTicker_go=true;};
void TimeTracker_act(){TimeTracker_go=true;};
//void sampleT_act(){sampleT_go=true;};

//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)
{   
    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();     //velocity or reference position change, EMG with a gain
    double biceps_r = (double) envelopeRout * EMGgain; //emg1.read();
    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;
        }

//    scope.set(0, inLout);
//    scope.set(1, inRout);  
    
}

void GetReferenceKinematics1(float &q1Out, float &q2Out, float &q1_refOut, float &q2_refOut){
    
    //get joint positions q feedback from encoder
    float Encoder1Position = counts1/resolution;         //angular position in radians, encoder axis
    float Encoder2Position = -1*counts2/resolution;     //NEGATIVE!
    
    q1Encoder = Encoder1Position*inverse_gear_ratio;
    q12Encoder = Encoder2Position*inverse_gear_ratio;
    q1Out = q1start + q1Encoder;        //angular position in radians, motor axis
    q12Out = q12start + q12Encoder;     //encoder 2 gives sum of both angles!
    q2Out = q12Out - q1Out;             
    float q1deg = q1Out*360/2/PI;
    float q2deg = q2Out*360/2/PI;
    
    /*
    //get end effector position feedback with Brockett
    float Position_x = ((L2 + x0)*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - L0*sin(q1) + (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) - cos(q1)*(L1*sin(q1) + L1*cos(q1)*sin(q2) - L1*cos(q2)*sin(q1)) - sin(q1)*(L1*cos(q1)*cos(q2) - L1*cos(q1) + L1*sin(q1)*sin(q2)));      //calculate end effector x-position from motor angles with Brockett, rx
    float Position_y = (L0 - (L2 + x0)*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) - L0*cos(q1) - cos(q1)*(L1*cos(q1)*cos(q2) - L1*cos(q1) + L1*sin(q1)*sin(q2)) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*(L1*sin(q1) + L1*cos(q1)*sin(q2) - L1*cos(q2)*sin(q1)));      //calculate end effector y-position from motor angles with Brockett, ry
    */
    //get end effector position feedback with trigonometry
    Position_x = (L1*sin(q1) + L2*sin(q1+q2));
    Position_y = (L1*cos(q1) + L2*cos(q1+q2)) + TowerHeight - StampHeight;  
    //float PositionVector = sqrt(pow(Position_x,2)+pow(Position_y,2));
    
    /*
    if (Position_y < (0.5*TowerHeight)){
     wait(1.0); 
     ReferencePosition_ynew = L1 + TowerHeight - StampHeight;      //Reset vertical position after stamping
      }
    else{      
    */
    //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG  
    biceps_l = !button1.read() * EMGgain; //emg0.read();     //velocity or reference position change, EMG with a gain
    biceps_r = !button2.read() * EMGgain; //emg1.read();
    if (T == -2){
        //both arms activated: stamp moves down
        //led1 = 1;
        //led2 = 1;
        ReferencePosition_xnew = ReferencePosition_x;
        ReferencePosition_ynew = ReferencePosition_y - 0.015; //ReferencePosition_y - dy_stampdown;   //into stamping vertical position ~the stamp down action
        
        }
    else if (T==-1){
        //arm 1 activated, move left
        //led1 = 1;
        //led2 = 0;
        ReferencePosition_xnew = ReferencePosition_x - 0.0009; //biceps_l;
        ReferencePosition_ynew = y_stampup; //ReferencePosition_y;
        /*
        PositionError_x = ReferencePosition_x - Position_x;        //Position error in dx,dy
        PositionError_y = ReferencePosition_y - Position_y;        //Position error in dx,dy
        
        dx = PositionError_x;
        dy = PositionError_y;
        q1_dotOut = dy*(((x0 + L1*cos(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (x0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1))) - dx*(((L0 + L1*sin(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (L0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)));
        q2_dotOut = dy*((x0*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(L1*L1*pow(cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - ((x0 + L1*cos(q1))*(pow(L0,2) + pow(x0,2) + 1))/(pow(L1*cos(q1),2)) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - dx*((L0*(L0*L0+L1*sin(q1)*L0+x0*x0+L1*cos(q1)*x0+1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1))-((L0 + L1*sin(q1))*(L0*L0 + x0*x0 + 1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1)));
        */
        }
    else if (T==1){
        //arm 1 activated, move right
        //led1 = 0;
        //led2 = 1;
        ReferencePosition_xnew = ReferencePosition_x + 0.0009; //biceps_r;
        ReferencePosition_ynew = y_stampup; //ReferencePosition_y;
        /*PositionError_x = ReferencePosition_x - Position_x;        //Position error in dx,dy
        PositionError_y = ReferencePosition_y - Position_y;        //Position error in dx,dy
        
        dx = PositionError_x;
        dy = PositionError_y;
        q1_dotOut = dy*(((x0 + L1*cos(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (x0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1))) - dx*(((L0 + L1*sin(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (L0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)));
        q2_dotOut = dy*((x0*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(L1*L1*pow(cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - ((x0 + L1*cos(q1))*(pow(L0,2) + pow(x0,2) + 1))/(pow(L1*cos(q1),2)) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - dx*((L0*(L0*L0+L1*sin(q1)*L0+x0*x0+L1*cos(q1)*x0+1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1))-((L0 + L1*sin(q1))*(L0*L0 + x0*x0 + 1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1)));
*/
        }
    else{       //T==0
        //led1 = 0;
        //led2 = 0;
        //ReferencePosition_xnew = ReferencePosition_x;
        ReferencePosition_ynew = y_stampup;     //ReferencePosition_y;   //
        }
      //  }
    
    //check position boundaries
    if (ReferencePosition_xnew > maxStampDistance){
        ReferencePosition_x = maxStampDistance; // - 0.1;
        ReferencePosition_y = y_stampup;
        pc.printf("Target too far! \r\n");
        }
    else if (ReferencePosition_xnew < minStampDistance){
        ReferencePosition_x = minStampDistance; // + 0.1;
        ReferencePosition_y = y_stampup;
        pc.printf("Target too close! \r\n");
        }
    else if (ReferencePosition_ynew < y_stampdown){
        ReferencePosition_x = ReferencePosition_xnew; // + 0.1;
        ReferencePosition_y = y_stampdown;
        pc.printf("Target too close! \r\n");
        }
    else {
        ReferencePosition_x = ReferencePosition_xnew;        
        ReferencePosition_y = ReferencePosition_ynew;
        }
    
    float PointPositionArm2_x = ReferencePosition_x;
    float PointPositionArm2_y = ReferencePosition_y - TowerHeight + StampHeight;
    float PointVectorArm2 = sqrt(pow(PointPositionArm2_x,2)+pow(PointPositionArm2_y,2));
    
    //calculate reference joint angles for the new reference position
    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 angle boundaries
    if (q1_refOutNew < q1_refOutMin){
        q1_refOut = q1_refOutMin;
        pc.printf("\r\n Under q1 angle boundaries\r\n");
        }
    else if (q1_refOutNew > q1_refOutMax){
        q1_refOut = q1_refOutMax;
        pc.printf("\r\n Above q1 angle boundaries\r\n");
        }
    else {
        q1_refOut = q1_refOutNew;
        }
        
    if (q2_refOutNew < q2_refOutMin){
        q2_refOut = q2_refOutMin;
        pc.printf("\r\n Under q2 angle boundaries");
        }
    else if (q2_refOutNew > q2_refOutMax){
        q2_refOut = q2_refOutMax;
        pc.printf("\r\n Above q2 angle boundaries");
        }
    else {
        q2_refOut = q2_refOutNew;
        }        
    //update joint angles
    //q1Out = q1Out + q1_dotOut;  //in radians
    //q2Out = q2Out + q2_dotOut;
    
    pc.baud(115200);
    pc.printf("posX: %f ",Position_x);
    pc.printf("posY: %f ",Position_y);
    pc.printf("refX: %f ",ReferencePosition_xnew);
    pc.printf("refY: %f ",ReferencePosition_ynew);
    pc.printf("q1: %f   ", q1Out);
    pc.printf("q1ref: %f    ", q1_refOut);
    pc.printf("q2: %f   ", q2Out);
    pc.printf("q2ref: %f    ", q2_refOut);
    pc.printf("q1deg: %f   ", q1deg);
    pc.printf("q2deg: %f   \r\n", q2deg);
    
    scope.set(0, ReferencePosition_xnew);
    scope.set(1, ReferencePosition_ynew);
    scope.set(2, envelopeL);
    scope.set(3, envelopeR);
    scope.set(4, T);
    scope.send();
    /*
    pc.printf("dx:          %f \r\n", dx);
    pc.printf("dy:          %f \r\n", dy);
    pc.printf("q1:          %f \r\n", q1Out);
    pc.printf("q1_dot:      %f \r\n", q1_dotOut);
    pc.printf("q2:          %f \r\n", q2Out);
    pc.printf("q2_dot:      %f \r\n", q2_dotOut);
    
    pc.printf("Counts1:     %f \r\n", counts1);
    pc.printf("Encoder1:    %f \r\n", Encoder1Position);
    pc.printf("Motor1:      %f \r\n", q1Out);
    pc.printf("Counts2:    %f \r\n", counts2);
    pc.printf("Encoder2:    %f \r\n", Encoder2Position);
    pc.printf("Motor2:      %f \r\n", q2Out);
    */
    
    }
    

void FeedbackControl1(float q1_ref, float q2_ref, float q1, float q2, float &motorValue1Out, float &motorValue2Out){
    
    // linear feedback control
    float q1_error = q1_ref - q1; //referencePosition1 - Position1;             // proportional angular error in radians
    float q2_error = q2_ref - q2; //referencePosition1 - Position1;             // proportional angular error in radians

    float TotalError1 = pidf.step(q1_error);
    float TotalError2 = pidf.step(q2_error);
    
    /*
    float q1IntError = q1IntError + q1_error*t_sample;             // integrated error in radians
    float q2IntError = q2IntError + q2_error*t_sample;             // integrated error in radians
    //float maxKi = 0.2;
    float Ki = 0.04*Kp;  //0.01*Kp; //potMeter2.read();
    
    float q1DerivativeError = (q1_error - q1_error_prev)/t_sample;  // derivative of error in radians
    float q2DerivativeError = (q2_error_prev + q2_error)/t_sample;  // derivative of error in radians
    //float maxKd = 0.2;
    float Kd = 0.0;  //0.04*Kp; //potMeter2.read();
    
    //scope.set(0,referencePosition1);
    //scope.set(1,Position1);
    //scope.set(2,Ki);
    //scope.send();
    
    TotalError1 = q1_error * Kp + q1IntError * Ki + q1DerivativeError * Kd;         //total controller output in radians = motor input
    TotalError2 = q2_error * Kp + q2IntError * Ki + q2DerivativeError * Kd;         //total controller output in radians = motor input
    */
    /*
    if (fabs(TotalError1) < TotalErrorMin) {
        TotalError1=0;
        }
    else {
        TotalError1=TotalError1;
        }
    if (fabs(TotalError2) < TotalErrorMin) {
        TotalError2=0;
        }
    else {
        TotalError2=TotalError2;
        }
    */
    /*
    DerTotalError1 = (TotalError1 - TotalError1_prev)/t_sample;
    DerTotalError1 = (TotalError1 - TotalError1_prev)/t_sample;
    motorValue1Out = DerTotalError1/MotorGain;       
    motorValue2Out = DerTotalError2/MotorGain;       
    */
    
    motorValue1Out = Motor1ExtraGain*(TotalError1/MotorGain);       
    motorValue2Out = TotalError2/MotorGain;       
    
    /*
    scope.set(0,q1_ref);
    scope.set(1,q1);
    scope.set(2,q2_ref);
    scope.set(3,q2);
    scope.set(4,motorValue1Out);
    scope.set(5,motorValue2Out);
    scope.send();
    */
    /*
    pc.printf("E1: %f  ", q1_error);
    pc.printf("IE1: %f  ", q1IntError);
    pc.printf("DE1: %f  ", q2DerivativeError);
    pc.printf("E2: %f  ", q2_error);
    pc.printf("IE2: %f  ", q2IntError);
    pc.printf("DE2: %f  ", q2DerivativeError);
    */
    //pc.printf("TE1: %f  ", TotalError1);
    //pc.printf("TE2: %f  ", TotalError2);
    pc.printf("M1: %f   \r\n", motorValue1Out);
    pc.printf("M2: %f   \r\n", motorValue2Out);
      
    q1_error_prev = q1_error;
    q2_error_prev = q2_error;
    TotalError1_prev = TotalError1;
    TotalError2_prev = TotalError2;
}

void SetMotor1(float motorValue1, float motorValue2)
{
    // Given -1<=motorValue<=1, this sets the PWM and direction
    // bits for motor 1. Positive value makes motor rotating
    // clockwise. motorValues outside range are truncated to
    // within range
    //control motor 1
    if (motorValue1 >=0) //clockwise rotation
        {motor1DirectionPin=cw;        //inverted due to opposite (to other motor) build-up in tower
        //led1=1;
        //led2=0;
        }
    else    //counterclockwise rotation 
        {motor1DirectionPin=ccw;         //inverted due to opposite (to other motor) build-up in tower
        //led1=0;
        //led2=1;
        }
    if (fabs(motorValue1)>MotorMaxSpeed){
        motor1MagnitudePin = MotorMaxSpeed;
        }
    else{
        motor1MagnitudePin = fabs(motorValue1);  //fabs(motorValue1);
        }
    
    //control motor 2
    if (motorValue2 >=0)  //clockwise rotation
        {motor2DirectionPin=cw; //action is ccw, due to faulty motor2DirectionPin (inverted)
        //led1=1;
        //led2=0;
        }
    else    //counterclockwise rotation 
        {motor2DirectionPin=ccw;    //action is cw, due to faulty motor2DirectionPin (inverted)
        //led1=0;
        //led2=1;
        }
    if (fabs(motorValue2)>MotorMaxSpeed){
        motor2MagnitudePin = MotorMaxSpeed;
        }
    else{
        motor2MagnitudePin = fabs(motorValue2);
        }
        float ReadDir1 = motor1DirectionPin.read();
        float ReadDir2 = motor2DirectionPin.read();
        pc.printf("M1 dir: %f \r\n", ReadDir1);
        pc.printf("M2 dir: %f \r\n", ReadDir2);
}

void MeasureAndControl()
{
    // This function measures the EMG of both arms, calculates via IK what
    // the joint 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);
}

void TimeTrackerF(){
     //wait(1);   
     //float Potmeter1 = potMeter1.read();
     //float referencePosition1 = GetReferencePosition();
     //pc.printf("TTReference Position: %d rad \r\n", referencePosition1);
     //pc.printf("TTPotmeter1, for refpos: %f \r\n", Potmeter1);
     //pc.printf("TTPotmeter2, Kp: %f \r\n", Potmeter2);
     //pc.printf("TTCounts: %i \r\n", counts1);
}

/*
void BiQuadFilter(){            //this function creates a BiQuad filter for the DerivativeCounts
    //double in=DerivativeCounts();
    bqcDerivativeCounts=bqc.step(DerivativeCounts);
    //return(bqcDerivativeCounts);
    }
*/  

int main()
{
 //Initialize
 //int led1val = led1.read();
 //int led2val = led2.read();
 pc.baud(115200);
 pc.printf("Test putty IK");
     ledRed=1;
    ledBlue=1; 
    ledRed=0; //red

    bcq1R.add(&bq1R).add(&bq2R);        //set BiQuad chains
    bcq2R.add(&bq3R);
    
    bcq1L.add(&bq1L).add(&bq2L);
    bcq2L.add(&bq3L);

            counts1 = Encoder1.getPulses();           // gives position of encoder 
            counts2 = Encoder2.getPulses();           // gives position of encoder 
 wait(20.0);
 MeasureTicker.attach(&MeasureTicker_act, 0.002); //0.1f); 
 //bqc.add(&bq1).add(&bq2);                   //set BiQuad chain
 pidf.PIDF( Kp, Ki, Kd, N, t_sample );    //set PID filter
 while(1)
    {
        if (MeasureTicker_go){
            MeasureTicker_go=false;
            ledGrn = 1;
            ledBlue = 0;
            MeasureAndControl();
            counts1 = Encoder1.getPulses();           // gives position of encoder 
            counts2 = Encoder2.getPulses();           // gives position of encoder 
            //pc.printf("counts1: %i  ", counts1);
            //pc.printf("counts2: %i  \r\n", counts2);
            ledBlue = 1;
            ledGrn = 0;
            }
/*
        if (BiQuadTicker_go){
            BiQuadTicker_go=false;
            BiQuadFilter();
        }
  */  
    }
}