All working, but Position_x and Position_y don't start at value 0, which causes spontaneous motor action.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of prog_inversekin by Gerhard Berman

Files at this revision

API Documentation at this revision

Comitter:
GerhardBerman
Date:
Wed Oct 19 13:28:38 2016 +0000
Parent:
6:3c4f3f2ce54f
Child:
8:935abf8ecc27
Commit message:
First trial for Inverse kinematics Feedforward implementation. No errors, not yet tested with board.

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
--- 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);
             }
 /*