Diff: main.cpp

Revision:

19:cba54636bd62

Parent:

18:d2cfd07ae88a

--- a/main.cpp	Mon Oct 24 09:07:07 2016 +0000
+++ b/main.cpp	Tue Oct 25 13:37:22 2016 +0000
@@ -13,9 +13,10 @@
  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 boundaries!!
+- Set angle and length boundaries!!
 - Set robot constants (lengths etc.)
 - Set EMGgain and thresholds
+- Add tower height to ReferencePosition_y and Position_y
 */
 
 //set pins
@@ -25,8 +26,8 @@
 DigitalIn encoder2B (D10); //Channel B van Encoder 2, kan niet op D14
 //DigitalOut led1 (D11); 
 //DigitalOut led2 (D10);
-AnalogIn potMeter1(A2);
-AnalogIn potMeter2(A1);
+//AnalogIn potMeter1(A2);
+//AnalogIn potMeter2(A1);
 DigitalOut motor1DirectionPin(D7);
 PwmOut motor1MagnitudePin(D6);
 DigitalOut motor2DirectionPin(D4);
@@ -39,31 +40,6 @@
 Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
 //HIDScope    scope(4);
 
-//set initial conditions
-float biceps_l = 0;
-float biceps_r = 0;
-float ReferencePosition_x = 0;
-float ReferencePosition_y = 0;
-float Position_x = 0;
-float Position_y = 0;
-float PositionError_x = 0;
-float PositionError_y = 0;
-float error1_prev = 0;
-float error2_prev = 0;
-float IntError1 = 0;
-float IntError2 = 0;
-float q1 = 0;
-float q2 = 0;
-//set initial conditions for function references
- float q1_dot = 0.0;
- float q2_dot = 0.0;
- float motorValue1 = 0.0;
- float motorValue2 = 0.0;
-int counts1 = 0;
-int counts2 = 0;
-int counts1Prev = 0;
-int counts2Prev = 0;
- 
 //set constant or variable values
 float EMGgain = 1.0;
 double DerivativeCounts;
@@ -73,15 +49,53 @@
 float L2 = 1.0;
 float dx;
 float dy;
-float dy_stampdown = 0.05; //5 cm movement downward to stamp
-
+float dy_stampdown = 2.0; //0.05; //5 cm movement downward to stamp
+float motorMaxGain1 = 0.1;
+float motorMaxGain2 = 0.1;
 float t_sample = 0.01; //seconds
-float referenceVelocity = 0;
-float bqcDerivativeCounts = 0;
+//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;
+const float maxStampDistance = 1.5;
 
+//set initial conditions
+float biceps_l = 0;
+float biceps_r = 0;
+
+float ReferencePosition_x = L2;
+float ReferencePosition_y = L1;
+float ReferencePosition_xnew = 0;
+float ReferencePosition_ynew = 0;
+float Position_x = 0.0;
+float Position_y = 0.0;
+float q1 = 0;
+float q2 = 0;
+float q1_ref = 0;
+float q2_ref = 0;
+float q1start = 0;
+float q2start = PI/2;
+
+float q1_refOutNew = 0;
+float q1_refOutMin = 0;         //WRONG values
+float q1_refOutMax = PI;        //WRONG values
+float q2_refOutNew = 0;
+float q2_refOutMin = 0;         //WRONG values
+float q2_refOutMax = PI;        //WRONG values
+
+float q1_error_prev = 0;
+float q2_error_prev = 0;
+float q1IntError = 0;
+float q2IntError = 0;
+
+float motorValue1 = 0.0;
+float motorValue2 = 0.0;
+int counts1 = 0;
+int counts2 = 0;
+int counts1Prev = 0;
+int counts2Prev = 0;
+ 
 //set BiQuad
 BiQuadChain bqc;
 BiQuad bq1(0.0186,    0.0743,    0.1114,    0.0743,    0.0186); //get numbers from butter filter MATLAB
@@ -101,30 +115,36 @@
 
 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
 
-void GetReferenceKinematics1(float &q1Out, float &q2Out, float &q1_dotOut, float &q2_dotOut){
+void GetReferenceKinematics1(float &q1Out, float &q2Out, float &q1_refOut, float &q2_refOut){
     
-    //get joint positions q from encoder
-    float Encoder1Position = counts1/resolution;         //position in radians, encoder axis
+    //get joint positions q feedback from encoder
+    float Encoder1Position = counts1/resolution;         //angular position in radians, encoder axis
     float Encoder2Position = counts2/resolution;
+
+    q1Out = q1start + Encoder1Position*inverse_gear_ratio;        //angular position in radians, motor axis
+    q2Out = q2start + Encoder2Position*inverse_gear_ratio;
     
-    q1Out = Encoder1Position*inverse_gear_ratio;        //position in radians, motor axis
-    q2Out = Encoder2Position*inverse_gear_ratio;
+    /*
+    //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));  
+    //float PositionVector = sqrt(pow(Position_x,2)+pow(Position_y,2));
       
     //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG  
-    biceps_l = EMGgain * !button1.read(); //emg0.read();     //velocity or reference position change, EMG with a gain
-    biceps_r = EMGgain * !button2.read(); //emg1.read();
+    biceps_l = !button1.read() * EMGgain; //emg0.read();     //velocity or reference position change, EMG with a gain
+    biceps_r = !button2.read() * EMGgain; //emg1.read();
     if (biceps_l > 0 && biceps_r > 0){
         //both arms activated: stamp moves down
         //led1 = 1;
         //led2 = 1;
-        ReferencePosition_x = ReferencePosition_x;
-        ReferencePosition_y = ReferencePosition_y - dy_stampdown;   //into stamping vertical position ~the stamp down action
+        ReferencePosition_xnew = ReferencePosition_x;
+        ReferencePosition_ynew = ReferencePosition_y - dy_stampdown;   //into stamping vertical position ~the stamp down action
         
         /*
          wait(1);   //lift stamp after stamping
@@ -139,8 +159,8 @@
         //arm 1 activated, move left
         //led1 = 1;
         //led2 = 0;
-        ReferencePosition_x = ReferencePosition_x - biceps_l;
-        ReferencePosition_y = ReferencePosition_y;
+        ReferencePosition_xnew = ReferencePosition_x - 0.2; //biceps_l;
+        ReferencePosition_ynew = ReferencePosition_y;
         /*
         PositionError_x = ReferencePosition_x - Position_x;        //Position error in dx,dy
         PositionError_y = ReferencePosition_y - Position_y;        //Position error in dx,dy
@@ -155,8 +175,8 @@
         //arm 1 activated, move right
         //led1 = 0;
         //led2 = 1;
-        ReferencePosition_x = ReferencePosition_x + biceps_r;
-        ReferencePosition_y = ReferencePosition_y;
+        ReferencePosition_xnew = ReferencePosition_x + 0.2; //biceps_r;
+        ReferencePosition_ynew = ReferencePosition_y;
         /*PositionError_x = ReferencePosition_x - Position_x;        //Position error in dx,dy
         PositionError_y = ReferencePosition_y - Position_y;        //Position error in dx,dy
         
@@ -169,38 +189,64 @@
     else{
         //led1 = 0;
         //led2 = 0;
+        ReferencePosition_xnew = ReferencePosition_x;
+        ReferencePosition_ynew = ReferencePosition_y;
+        }
+    
+    float ReferenceVector = sqrt(pow(ReferencePosition_x,2)+pow(ReferencePosition_y,2));
+    
+    //check position boundaries
+    if (ReferenceVector > maxStampDistance){
         ReferencePosition_x = ReferencePosition_x;
         ReferencePosition_y = ReferencePosition_y;
         }
-            
-    //calculate joint angle differences
-    PositionError_x = ReferencePosition_x - Position_x;        //Position error in dx,dy
-    PositionError_y = ReferencePosition_y - Position_y;        //Position error in dx,dy
+    else if (ReferencePosition_ynew < 0){
+        ReferencePosition_y = 0;            //could also be little negative value to get more pressure on table
+        }
+    else {
+        ReferencePosition_x = ReferencePosition_xnew;        
+        ReferencePosition_y = ReferencePosition_ynew;
+    
+    //calculate reference joint angles for the new reference position
+    float alpha = atan(ReferencePosition_y/ReferencePosition_x);
+    float beta = acos((L2*L2-L1*L1-pow(ReferenceVector,2))/(-2*L1*ReferenceVector));
+    q1_refOutNew = PI/2 - (alpha+beta);
+    q2_refOutNew = PI - asin(ReferenceVector*sin(beta)/L2);
+    
+    //check angle boundaries
+    if (q1_refOutNew > q1_refOutMin && q1_refOutMax < q1_refOutMax){
+        q1_refOut = q1_refOutNew;
+        }
+    else {
+        q1_refOut = q1_refOut;
+        }
+    if (q2_refOutNew > q2_refOutMin && q2_refOutMax < q2_refOutMax){
+        q2_refOut = q2_refOutNew;
+        }
+    else {
+        q2_refOut = q2_refOut;
+        }
         
-    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)));
-
     
     //update joint angles
     //q1Out = q1Out + q1_dotOut;  //in radians
     //q2Out = q2Out + q2_dotOut;
     
     pc.baud(115200);
-    pc.printf("Reference_x:     %f \r\n", ReferencePosition_x);
-    pc.printf("Position_x:      %f \r\n", Position_x);
-    pc.printf("PositionError_x: %f \r\n", PositionError_x);
-    pc.printf("dx:              %f \r\n", dx);
-    pc.printf("q1dotOut:        %f \r\n", q1_dotOut);
-    pc.printf("q1Out:           %f \r\n", q1Out);
+    pc.printf("biceps_l:        %f \r\n", biceps_l);
+    pc.printf("biceps_r:        %f \r\n", biceps_r);
+    pc.printf("Position_x:      %f m\r\n", Position_x);
+    pc.printf("q1Out:           %f rad \r\n", q1Out);
+    pc.printf("Reference_x:     %f m \r\n", ReferencePosition_x);
+    pc.printf("q1refOut:        %f rad\r\n", q1_refOut);
+    pc.printf("Position_y:      %f m\r\n", Position_y);
+    pc.printf("q2Out:           %f rad \r\n", q2Out);
+    pc.printf("Reference_y:     %f m \r\n", ReferencePosition_y);
+    pc.printf("q2refOut:        %f rad\r\n", q2_refOut);
+    pc.printf("alpha:           %f rad \r\n", alpha);
+    pc.printf("beta:            %f rad\r\n", beta);
     
-    pc.printf("Reference_y:     %f \r\n", ReferencePosition_y);
-    pc.printf("Position_y:      %f \r\n", Position_y);
-    pc.printf("PositionError_y: %f \r\n", PositionError_y);
-    pc.printf("dy:              %f \r\n", dy);
-    pc.printf("q2dotOut:        %f \r\n", q2_dotOut);
-    pc.printf("q2Out:           %f \r\n", q2Out);
+    
     /*
     pc.printf("dx:          %f \r\n", dx);
     pc.printf("dy:          %f \r\n", dy);
@@ -219,22 +265,22 @@
     }
     
 
-void FeedForwardControl1(float q1_dot, float q2_dot, float &motorValue1Out, float &motorValue2Out){
+void FeedForwardControl1(float q1_ref, float q2_ref, float q1, float q2, float &motorValue1Out, float &motorValue2Out){
     //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 angular error in radians
-    float error2 = q2_dot; //referencePosition1 - Position1;             // proportional angular error in radians
+    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 Kp = 1; //potMeter2.read();
 
-    float IntError1 = IntError1 + error1*t_sample;             // integrated error in radians
-    float IntError2 = IntError2 + error2*t_sample;             // integrated error in radians
+    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.1; //potMeter2.read();
     
-    float DerivativeError1 = (error1_prev + error1)/t_sample;  // derivative of error in radians
-    float DerivativeError2 = (error2_prev + error2)/t_sample;  // derivative of error in radians
+    float q1DerivativeError = (q1_error_prev + q1_error)/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; //potMeter2.read();
     
@@ -243,22 +289,22 @@
     //scope.set(2,Ki);
     //scope.send();
     
-    motorValue1Out = error1 * Kp + IntError1 * Ki + DerivativeError1 * Kd;    //total controller output = motor input
-    motorValue2Out = error2 * Kp + IntError2 * Ki + DerivativeError2 * Kd;    //total controller output = motor input
+    motorValue1Out = q1_error * Kp + q1IntError * Ki + q1DerivativeError * Kd;    //total controller output = motor input
+    motorValue2Out = q2_error * Kp + q2IntError * Ki + q2DerivativeError * 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);
     
-    pc.printf("error1: %f \r\n", error1);
-    pc.printf("IntError1: %f \r\n", IntError1);
-    pc.printf("DerError1: %f \r\n", DerivativeError1);
-    pc.printf("error2: %f \r\n", error2);
-    pc.printf("IntError2: %f \r\n", IntError2);
-    pc.printf("DerError2: %f \r\n", DerivativeError2);
+    pc.printf("error1: %f \r\n", q1_error);
+    pc.printf("IntError1: %f \r\n", q1IntError);
+    pc.printf("DerError1: %f \r\n", q1DerivativeError);
+    pc.printf("error2: %f \r\n", q2_error);
+    pc.printf("IntError2: %f \r\n", q2IntError);
+    pc.printf("DerError2: %f \r\n", q2DerivativeError);
         
-    error1_prev = error1;
-    error2_prev = error1;
+    q1_error_prev = q1_error;
+    q2_error_prev = q2_error;
     //float biceps_l = !button1.read();
     //float biceps_r = !button2.read();
        
@@ -279,17 +325,17 @@
     // within range
     //control motor 1
     if (motorValue1 >=0) //clockwise rotation
-        {motor1DirectionPin=ccw;        //inverted due to opposite (to other motor) build-up in tower
+        {motor1DirectionPin=cw;        //inverted due to opposite (to other motor) build-up in tower
         //led1=1;
         //led2=0;
         }
     else    //counterclockwise rotation 
-        {motor1DirectionPin=cw;         //inverted due to opposite (to other motor) build-up in tower
+        {motor1DirectionPin=ccw;         //inverted due to opposite (to other motor) build-up in tower
         //led1=0;
         //led2=1;
         }
-    if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
-        else motor1MagnitudePin = 0.1*fabs(motorValue1);  //fabs(motorValue1);
+    if (fabs(motorValue1)>1) motor1MagnitudePin = motorMaxGain1;
+        else motor1MagnitudePin = motorMaxGain1*fabs(motorValue1);  //fabs(motorValue1);
     //control motor 2
     if (motorValue2 >=0)  //clockwise rotation
         {motor2DirectionPin=ccw; //action is cw, due to faulty motor2DirectionPin (inverted)
@@ -301,8 +347,12 @@
         //led1=0;
         //led2=1;
         }
-    if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
-        else motor2MagnitudePin = 0.1*fabs(motorValue2);  //fabs(motorValue1);
+    if (fabs(motorValue2)>1) motor2MagnitudePin = motorMaxGain2;
+        else motor2MagnitudePin = motorMaxGain2*fabs(motorValue2);  //fabs(motorValue1);
+        float ReadMagn1 = motor1MagnitudePin.read();
+        float ReadMagn2 = motor2MagnitudePin.read();
+        pc.printf("motor1Magnitude: %f \r\n", ReadMagn1);
+        pc.printf("motor2Magnitude: %f \r\n", ReadMagn2);
 }
 
 void MeasureAndControl()
@@ -310,8 +360,8 @@
     // This function measures the EMG of both arms, calculates via IK what
     // the joint speeds should be, and controls the motor with 
     // a Feedforward controller. This is called from a Ticker.
-    GetReferenceKinematics1(q1, q2, q1_dot, q2_dot);
-    FeedForwardControl1( q1_dot, q2_dot, motorValue1, motorValue2);
+    GetReferenceKinematics1(q1, q2, q1_ref, q2_ref);
+    FeedForwardControl1( q1_ref, q2_ref, q1, q2, motorValue1, motorValue2);
     SetMotor1(motorValue1, motorValue2);
 }