Diff: main.cpp

Revision:

7:2f74dfd1d411

Parent:

6:3c4f3f2ce54f

Child:

8:935abf8ecc27

--- a/main.cpp	Mon Oct 17 14:34:25 2016 +0000
+++ b/main.cpp	Wed Oct 19 13:28:38 2016 +0000
@@ -14,27 +14,43 @@
 AnalogIn potMeter2(A1);
 DigitalOut motor1DirectionPin(D7);
 PwmOut motor1MagnitudePin(D6);
-DigitalIn button1(D5);
+DigitalOut motor2DirectionPin(D4);
+PwmOut motor2MagnitudePin(D5);
+DigitalIn button1(D8);
+DigitalIn button2(D9);
 
-//set settings
+//library settings
 Serial pc(USBTX,USBRX);
 Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
 HIDScope    scope(3);
 
-//set datatypes
-int counts = 0;
-double DerivativeCounts;
+//set initial conditions
+float error1_prev = 0;
+float error2_prev = 0;
+float IntError1 = 0;
+float IntError2 = 0;
+float q1 = 0;
+float q2 = 0;
+float q1_dot;
+float q2_dot;
 
-float error_prev = 0;
-float IntError = 0;
+//set constant or variable values
+int counts1 = 0;
+int counts2 = 0;
+int counts1Prev = 0;
+int counts2Prev = 0;
+double DerivativeCounts;
+float x0 = 1.0;
+float L0 = 1.0;
+float L1 = 1.0;
+float dx;
+float dy;
+float dy_stampdown = 0.05; //5 cm movement downward to stamp
+
 float t_sample = 0.01; //seconds
-
-int countsPrev = 0;
 float referenceVelocity = 0;
 float bqcDerivativeCounts = 0;
 const float PI = 3.141592653589793;
-//float Potmeter1 = potMeter1.read();
-//float Potmeter2 = potMeter2.read();
 const int cw = 0;       //values for cw and ccw are inverted!! cw=0 and ccw=1
 const int ccw = 1;
 
@@ -52,66 +68,182 @@
 //void sampleT_act(){sampleT_go=true;};
 
 //define encoder counts and degrees
-QEI Encoder(D12, D13, NC, 32); // turns on encoder
+QEI Encoder1(D12, D13, NC, 32); // turns on encoder
+QEI Encoder2(D14, D15, NC, 32); // turns on encoder
 const int counts_per_revolution = 4200; //counts per motor axis revolution
 const int inverse_gear_ratio = 131;
 //const float motor_axial_resolution = counts_per_revolution/(2*PI);
 const float resolution = counts_per_revolution/(2*PI/inverse_gear_ratio);  //87567.0496892 counts per radian, encoder axis
 
-float GetReferencePosition()
-{
+float GetReferenceKinematics1(){
+    
+    //get joint positions q from encoder
+    float Encoder1Position = counts1/resolution;         //position in radians, encoder axis
+    float q1 = Encoder1Position*inverse_gear_ratio;        //position in radians, motor axis
+    
+    //float Encoder2Position = counts2/resolution;         //position in radians, encoder axis
+    //float q2 = Encoder2Position*inverse_gear_ratio;        //position in radians, motor axis
+    
+    //NOTNECESSARY calculate end effector position with Brockett
+    
+    //NOTNECESSARY get desired position Pe* from EMG(?) 
+    
+    //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG  
+    float biceps1 = button1.read();
+    float biceps2 = button2.read();
+    while (biceps1 > 0){
+        if (biceps2 > 0){ //both arms activated: stamp moves down
+            dx = 0;
+            dy = dy_stampdown; //into stamping vertical position?? ~the stamp down action
+            wait(1);
+            dy = -(dy_stampdown);  //reset vertical position
+            }
+            else{           //left arm activated
+            dx = biceps1;
+            dy = 0;
+            }
+    while (biceps2 > 0){
+        if (biceps1 <= 0){  //right arm activated
+            dx = -biceps2;
+            dy = 0;
+            } 
+            }
+    
+    //get joint angles change q_dot = Jpseudo * TwistEndEff  (Matlab)
+    float q1_dot = 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)));
+
+    //update joint angles
+    q1 = q1 + q1_dot;
+    }
+    return q1_dot;
+    }
+
+float GetReferenceKinematics2(){
+    
+    //get joint positions q from encoder
+    float Encoder1Position = counts1/resolution;         //position in radians, encoder axis
+    float q1 = Encoder1Position*inverse_gear_ratio;        //position in radians, motor axis
+    
+    float Encoder2Position = counts2/resolution;         //position in radians, encoder axis
+    float q2 = Encoder2Position*inverse_gear_ratio;        //position in radians, motor axis
+    
+    //NOTNECESSARY calculate end effector position with Brockett
+    
+    //NOTNECESSARY get desired position Pe* from EMG(?) 
+    
+    //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG  
+    float biceps1 = button1.read();
+    float biceps2 = button2.read();
+    while (biceps1 > 0){
+        if (biceps2 > 0){ //both arms activated: stamp moves down
+            dx = 0;
+            dy = dy_stampdown; //into stamping vertical position?? ~the stamp down action
+            wait(1);
+            dy = -(dy_stampdown);  //reset vertical position
+            }
+            else{           //left arm activated
+            dx = biceps1;
+            dy = 0;
+            }
+    while (biceps2 > 0){
+        if (biceps1 <= 0){  //right arm activated
+            dx = -biceps2;
+            dy = 0;
+            } 
+            }
+    
+        //get joint angles change q_dot = Jpseudo * TwistEndEff;  (Matlab)
+    float q2_dot = 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)));
+    
+    //update joint angles
+    q2 = q2 + q2_dot;
+    }
+    return q2_dot;
+    }
+
+/*
+float GetReferencePosition(){
     // Returns reference position in rad. 
     // Positive value means clockwise rotation.
     const float maxPosition = 2*PI; //6.283185307179586; // in radians
     float Potmeter1 = potMeter1.read();
-    float referencePosition = Potmeter1 * maxPosition; //Potmeter1 * maxPosition; //refpos in radians 
+    float referencePosition1 = Potmeter1 * maxPosition; //Potmeter1 * maxPosition; //refpos in radians 
     pc.printf("Max Position: %f rad \r\n", maxPosition);
     pc.printf("Potmeter1, refpos: %f \r\n", Potmeter1);
-    pc.printf("Motor Axis Ref Position: %f rad \r\n", referencePosition);
-    return referencePosition;
+    pc.printf("Motor Axis Ref Position1: %f rad \r\n", referencePosition1);
+    return referencePosition1;
+}
+*/
+
+float FeedForwardControl1(float q1_dot){
+    //float Encoder1Position = counts1/resolution;         //position in radians, encoder axis
+    //float Position1 = Encoder1Position*inverse_gear_ratio;        //position in radians, motor axis
+    
+    // linear feedback control
+    float error1 = q1_dot; //referencePosition1 - Position1;             // proportional error in radians
+    float Kp = 1; //potMeter2.read();
+
+    float IntError1 = IntError1 + error1*t_sample;             // integrated error in radians
+    //float maxKi = 0.2;
+    float Ki = 0.1; //potMeter2.read();
+    
+    float DerivativeError1 = (error1_prev + error1)/t_sample;  // derivative of error in radians
+    //float maxKd = 0.2;
+    float Kd = 0.0; //potMeter2.read();
+    
+    //scope.set(0,referencePosition1);
+    //scope.set(1,Position1);
+    //scope.set(2,Ki);
+    //scope.send();
+    
+    float motorValue1 = error1 * Kp + IntError1 * Ki + DerivativeError1 * Kd;    //total controller output = motor input
+    //pc.printf("Motor Axis Position: %f rad \r\n", Position1);
+    //pc.printf("Counts encoder1: %i rad \r\n", counts1);
+    //pc.printf("Kp: %f \r\n", Kp);
+    //pc.printf("MotorValue: %f \r\n", motorValue1);
+    
+    error1_prev = error1;
+    return motorValue1;
 }
 
-float FeedForwardControl(float referencePosition)
-{
-    float EncoderPosition = counts/resolution;         //position in radians, encoder axis
-    float Position = EncoderPosition*inverse_gear_ratio;        //position in radians, motor axis
-    // linear feedback control
+float FeedForwardControl2(float q2_dot){
+    //float Encoder2Position = counts2/resolution;         //position in radians, encoder axis
+    //float Position2 = Encoder2Position*inverse_gear_ratio;        //position in radians, motor axis
     
-    float error = referencePosition - Position;             // proportional error in radians
+    // linear feedback control
+    float error2 = q2_dot; //referencePosition2 - Position2;             // proportional error in radians
     float Kp = 1; //potMeter2.read();
-    
-    float IntError = IntError + error*t_sample;             // integrated error in radians
-    float maxKi = 0.2;
-    float Ki = potMeter2.read(); //*maxKi;
+
+    float IntError2 = IntError2 + error2*t_sample;             // integrated error in radians
+    //float maxKi = 0.2;
+    float Ki = 0.1; //potMeter2.read();
     
-    /*
-    float DerivativeError = (error_prev + error)/t_sample;  // derivative of error in radians
-    float maxKd = 0.2;
-    float Kd = potMeter2.read()*maxKd;
-    */
+    float DerivativeError2 = (error2_prev + error2)/t_sample;  // derivative of error in radians
+    //float maxKd = 0.2;
+    float Kd = 0.0; //potMeter2.read()*maxKd;
+    
+    //scope.set(0,referencePosition1);
+    //scope.set(1,Position1);
+    //scope.set(2,Ki);
+    //scope.send();
     
-    scope.set(0,referencePosition);
-    scope.set(1,Position);
-    scope.set(2,Ki);
-    scope.send();
+    float motorValue2 = error2 * Kp + IntError2 * Ki + DerivativeError2 * Kd;    //total controller output = motor input
+    //pc.printf("Motor Axis Position: %f rad \r\n", Position1);
+    //pc.printf("Counts encoder1: %i rad \r\n", counts1);
+    //pc.printf("Kp: %f \r\n", Kp);
+    //pc.printf("MotorValue: %f \r\n", motorValue1);
     
-    float motorValue = error * Kp + IntError * Ki; // + DerivativeError * Kd;    //total controller output = motor input
-    pc.printf("Motor Axis Position: %f rad \r\n", Position);
-    pc.printf("Counts encoder: %i rad \r\n", counts);
-    pc.printf("Kp: %f \r\n", Kp);
-    pc.printf("MotorValue: %f \r\n", motorValue);
-    
-    error_prev = error;
-    return motorValue;
+    error2_prev = error2;
+    return motorValue2;
 }
 
-void SetMotor1(float motorValue)
+void SetMotor1(float motorValue1)
 {
     // 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
-    if (motorValue >=0) 
+    if (motorValue1 >=0) 
         {motor1DirectionPin=cw;
         led1=1;
         led2=0;
@@ -120,8 +252,27 @@
         led1=0;
         led2=1;
         }
-    if (fabs(motorValue)>1) motor1MagnitudePin = 1;
-        else motor1MagnitudePin = fabs(motorValue);
+    if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
+        else motor1MagnitudePin = fabs(motorValue1);
+}
+
+void SetMotor2(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
+    if (motorValue2 >=0) 
+        {motor2DirectionPin=cw;
+        led1=1;
+        led2=0;
+        }
+    else {motor2DirectionPin=ccw;
+        led1=0;
+        led2=1;
+        }
+    if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
+        else motor2MagnitudePin = fabs(motorValue2);
 }
 
 void MeasureAndControl()
@@ -129,20 +280,26 @@
     // This function measures the potmeter position, extracts a
     // reference position from it, and controls the motor with 
     // a Feedback controller. Call this from a Ticker.
-    float referencePosition = GetReferencePosition();
-    float motorValue = FeedForwardControl(referencePosition);
-    SetMotor1(motorValue);
+    float referencePosition1 = GetReferenceKinematics1();
+    float referencePosition2 = GetReferenceKinematics2();
+    //float referencePosition1 = GetReferencePosition1();
+    //float referencePosition2 = GetReferencePosition2();
+    float motorValue1 = FeedForwardControl1(referencePosition1);
+    float motorValue2 = FeedForwardControl2(referencePosition2);
+    SetMotor1(motorValue1);
+    SetMotor2(motorValue2);
 }
 
 void TimeTrackerF(){
      //wait(1);   
      //float Potmeter1 = potMeter1.read();
-     float referencePosition = GetReferencePosition();
-     pc.printf("TTReference Position: %d rad \r\n", referencePosition);
+     //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", counts);
+     //pc.printf("TTCounts: %i \r\n", counts1);
 }
+
 /*
 void BiQuadFilter(){            //this function creates a BiQuad filter for the DerivativeCounts
     //double in=DerivativeCounts();
@@ -167,7 +324,8 @@
         if (MeasureTicker_go){
             MeasureTicker_go=false;
             MeasureAndControl();
-            counts = Encoder.getPulses();           // gives position of encoder 
+            counts1 = Encoder1.getPulses();           // gives position of encoder 
+            counts2 = Encoder2.getPulses();           // gives position of encoder 
             pc.printf("Resolution: %f pulses/rad \r\n",resolution);
             }
 /*