Working, but boundaries not yet tested

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of prog_inversekin_feedback by Gerhard Berman

Files at this revision

API Documentation at this revision

Comitter:
GerhardBerman
Date:
Wed Oct 19 15:38:44 2016 +0000
Parent:
8:935abf8ecc27
Child:
10:45473f650198
Commit message:
Double functions (one for motor 1, one for motor 2) replaced by single. No errors, no reaction on board

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/main.cpp	Wed Oct 19 14:16:10 2016 +0000
+++ b/main.cpp	Wed Oct 19 15:38:44 2016 +0000
@@ -8,6 +8,8 @@
 //set pins
 DigitalIn encoder1A (D13); //Channel A van Encoder 1
 DigitalIn encoder1B (D12); //Channel B van Encoder 1
+DigitalIn encoder2A (D14);
+DigitalIn encoder2B (D15);
 DigitalOut led1 (D11); 
 DigitalOut led2 (D10);
 AnalogIn potMeter1(A2);
@@ -16,7 +18,7 @@
 PwmOut motor1MagnitudePin(D6);
 DigitalOut motor2DirectionPin(D4);
 PwmOut motor2MagnitudePin(D5);
-DigitalIn button1(D8);
+DigitalIn button1(D3);
 DigitalIn button2(D9);
 
 //library settings
@@ -31,9 +33,12 @@
 float IntError2 = 0;
 float q1 = 0;
 float q2 = 0;
-float q1_dot;
-float q2_dot;
-
+//set initial conditions for function references
+ float q1_dot = 0.0;
+ float q2_dot = 0.0;
+ float motorValue1 = 0.0;
+ float motorValue2 = 0.0;
+ 
 //set constant or variable values
 int counts1 = 0;
 int counts2 = 0;
@@ -75,24 +80,22 @@
 //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 GetReferenceKinematics1(){
+void GetReferenceKinematics1(float &q1_dotOut, float &q2_dotOut){
     
     //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
+    float Encoder2Position = counts2/resolution;
     
-    //NOTNECESSARY calculate end effector position with Brockett
-    
-    //NOTNECESSARY get desired position Pe* from EMG(?) 
-    
+    float q1 = Encoder1Position*inverse_gear_ratio;        //position in radians, motor axis
+    float q2 = Encoder2Position*inverse_gear_ratio;
+      
     //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG  
     float biceps1 = button1.read();
     float biceps2 = button2.read();
     if (biceps1 > 0 && biceps2 > 0){
         //both arms activated: stamp moves down
+        //led1 = 1;
+        //led2 = 1;
         dx = 0;
         dy = dy_stampdown; //into stamping vertical position?? ~the stamp down action
          wait(1);
@@ -100,97 +103,51 @@
         }
     else if (biceps1 > 0 && biceps2 <= 0){
         //arm 1 activated, move left
+        //led1 = 1;
+        //led2 = 0;
         dx = -biceps1;
         dy = 0;
         }
     else if (biceps1 <= 0 && biceps2 > 0){
         //arm 1 activated, move left
+        //led1 = 0;
+        //led2 = 1;
         dx = biceps2;
         dy = 0;
         }
     else{
+        //led1 = 0;
+        //led2 = 0;
         dx=0;
         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)));
+    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
-    q1 = q1 + q1_dot;
-    return q1_dot;
+    q1 = q1 + q1_dotOut;
+    q2 = q2 + q2_dotOut;
+    
     }
 
-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 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 Position1: %f rad \r\n", referencePosition1);
-    return referencePosition1;
-}
-*/
-
-float FeedForwardControl1(float q1_dot){
+void FeedForwardControl1(float q1_dot, float q2_dot, 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 error in radians
+    float error2 = q2_dot; //referencePosition1 - Position1;             // proportional 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 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 maxKd = 0.2;
     float Kd = 0.0; //potMeter2.read();
     
@@ -199,72 +156,36 @@
     //scope.set(2,Ki);
     //scope.send();
     
-    float motorValue1 = error1 * Kp + IntError1 * Ki + DerivativeError1 * Kd;    //total controller output = motor input
+    motorValue1Out = error1 * Kp + IntError1 * Ki + DerivativeError1 * Kd;    //total controller output = motor input
+    motorValue2Out = 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);
     
     error1_prev = error1;
-    return motorValue1;
+    error2_prev = error1;
 }
 
-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
-    
-    // linear feedback control
-    float error2 = q2_dot; //referencePosition2 - Position2;             // proportional error in radians
-    float Kp = 1; //potMeter2.read();
-
-    float IntError2 = IntError2 + error2*t_sample;             // integrated error in radians
-    //float maxKi = 0.2;
-    float Ki = 0.1; //potMeter2.read();
-    
-    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();
-    
-    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);
-    
-    error2_prev = error2;
-    return motorValue2;
-}
-
-void SetMotor1(float motorValue1)
+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) 
         {motor1DirectionPin=cw;
-        led1=1;
-        led2=0;
+        //led1=1;
+        //led2=0;
         }
     else {motor1DirectionPin=ccw;
-        led1=0;
-        led2=1;
+        //led1=0;
+        //led2=1;
         }
     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
+    //control motor 2
     if (motorValue2 >=0) 
         {motor2DirectionPin=cw;
         led1=1;
@@ -280,17 +201,12 @@
 
 void MeasureAndControl()
 {
-    // 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 referencePosition1 = GetReferenceKinematics1();
-    float referencePosition2 = GetReferenceKinematics2();
-    //float referencePosition1 = GetReferencePosition1();
-    //float referencePosition2 = GetReferencePosition2();
-    float motorValue1 = FeedForwardControl1(referencePosition1);
-    float motorValue2 = FeedForwardControl2(referencePosition2);
-    SetMotor1(motorValue1);
-    SetMotor2(motorValue2);
+    // 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_dot, q2_dot);
+    FeedForwardControl1( q1_dot, q2_dot, motorValue1, motorValue2);
+    SetMotor1(motorValue1, motorValue2);
 }
 
 void TimeTrackerF(){
@@ -309,19 +225,24 @@
     bqcDerivativeCounts=bqc.step(DerivativeCounts);
     //return(bqcDerivativeCounts);
     }
-  */  
+*/  
 
 int main()
 {
  //Initialize
- led1=1;
- led2=1;
- pc.baud(115200);
+  pc.baud(115200);
  pc.printf("Test putty");
- MeasureTicker.attach(&MeasureTicker_act, 0.01f); 
- bqc.add(&bq1).add(&bq2);
- QEI Encoder(D12, D13, NC, 32); // turns on encoder
- 
+ led1=1;
+    led1 = 0;
+    led2 = 0;
+    wait(2.0);
+    led1 = 1;
+    led2 = 1;
+    wait(2.0);
+ MeasureTicker.attach(&MeasureTicker_act, 1.0f); 
+ //bqc.add(&bq1).add(&bq2);
+ //QEI Encoder1(D12, D13, NC, 32); // turns on encoder
+ /*
  while(1)
     {
         if (MeasureTicker_go){
@@ -331,11 +252,12 @@
             counts2 = Encoder2.getPulses();           // gives position of encoder 
             pc.printf("Resolution: %f pulses/rad \r\n",resolution);
             }
-/*
+
         if (BiQuadTicker_go){
             BiQuadTicker_go=false;
             BiQuadFilter();
         }
-*/    
+    
     }
+*/
 }
\ No newline at end of file