Diff: main.cpp

Revision:

46:4af2f01da9f3

Parent:

45:0f91abd76b93

Child:

47:13b4a318a3d0

--- a/main.cpp	Sat Nov 05 16:02:57 2016 +0000
+++ b/main.cpp	Sat Nov 05 17:13:23 2016 +0000
@@ -4,7 +4,7 @@
 #include "QEI.h"
 #include "HIDScope.h"
 #include "BiQuad.h"
-
+ 
 /*
 THINGS TO CONSIDER
 - Motor action of motor 1 is inverted because it is mounted
@@ -13,17 +13,17 @@
  in the original IK-sketch.
 - Robot arms should be placed manually into reference position before resetting board.
 */
-
+ 
 //set pins
-DigitalIn encoder1A (D13);                                                  //Channel A from Encoder 1
-DigitalIn encoder1B (D12);                                                  //Channel B from Encoder 1
-DigitalIn encoder2A (D11);                                                  //Channel A from Encoder 2
-DigitalIn encoder2B (D10);                                                  //Channel B from Encoder 2
+DigitalIn encoder1A (D13);                          //Channel A from Encoder 1
+DigitalIn encoder1B (D12);                              //Channel B from Encoder 1
+DigitalIn encoder2A (D11);                                  //Channel A from Encoder 2
+DigitalIn encoder2B (D10);                                      //Channel B from Encoder 2
 AnalogIn emg0( A0 );
 AnalogIn emg1( A1 );
 DigitalIn button1(D3);
 DigitalIn button2(D9);
-
+ 
 DigitalOut motor1DirectionPin(D7);
 PwmOut motor1MagnitudePin(D6);
 DigitalOut motor2DirectionPin(D4);
@@ -31,56 +31,56 @@
 DigitalOut ledGrn(LED_GREEN);
 DigitalOut ledRed(LED_RED);
 DigitalOut ledBlue(LED_BLUE);
-
+ 
 //library settings
 Serial pc(USBTX,USBRX);
 HIDScope    scope(6);
-
+ 
 //go-Ticker settings
 Ticker MeasureTicker;
 volatile bool MeasureTicker_go=false;
-void MeasureTicker_act(){MeasureTicker_go=true;};                           // Activates go-flag
-
+void MeasureTicker_act(){MeasureTicker_go=true;};               // Activates go-flag
+ 
 //constant values
 const float PI = 3.141592653589793;
-const int cw = 0;                                                           //values for cw and ccw are inverted! cw=0 and ccw=1
+const int cw = 0;               //values for cw and ccw are inverted! cw=0 and ccw=1
 const int ccw = 1;
-double threshold_l=0.09;                                                    //left arm EMG threshold
-double threshold_r = 0.08;                                                  //right arm EMG threshold
+double threshold_l=0.09;                                        //left arm EMG threshold
+double threshold_r = 0.08;                                      //right arm EMG threshold
 float EMGgain = 1.0;
-
+ 
 //set lengths
-//float L0 = 0.232;                                                           //height of motor axes above table surface
-float L1 = 0.45;                                                            //length of proximal arm
-float L2 = 0.35;                                                            //length of distal arm
-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
-
-//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
-float MotorMaxSpeed = 0.1;                                                  //define a maximum PWM speed for the motor
-float t_sample = 0.002;                                                     //seconds
+//float L0 = 0.232;                     //height of motor axes above table surface
+float L1 = 0.45;                        //length of proximal arm
+float L2 = 0.35;                            //length of distal arm
+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
+ 
+float MotorGain = 8.4;          // rad/s for PWM, is max motor speed
+float MotorMaxSpeed = 0.1;                  //define a maximum PWM speed for the motor
+float t_sample = 0.002;                                     //seconds
 const float maxStampDistance = 0.7;
 const float minStampDistance = 0.3;
-float Kp = 4.0;                                                             //proportional controller constant
-float Ki = 0.04;                                                            //integrative controller constant
-float Kd = 0.02;                                                            //derivative controller constant
-float N = 25;                                                               //PIDF filter constant
+float Kp = 4.0;                          //proportional controller constant
+float Ki = 0.04;                            //integrative controller constant
+float Kd = 0.02;                        //derivative controller constant
+float N = 25;                               //PIDF filter constant
 //(Higher N is faster derivative action but more sensitive to noise)
  
 //set initial conditions for inputs and positions
 float biceps_l = 0, biceps_r = 0;
 double envelopeL = 0, envelopeR = 0;
-int T=0;                                                                    //EMG 'switch' variable
-float ReferencePosition_x = 0.35;                                           //starting position for x reference position        
-float ReferencePosition_y = L1 + TowerHeight - StampHeight;                 //starting position for y reference position
+int T=0;                                //EMG 'switch' variable
+float ReferencePosition_x = 0.35;       //starting position for x reference position        
+float ReferencePosition_y = L1 + TowerHeight - StampHeight; 
+//starting position for y reference position
 float ReferencePosition_xnew = 0.35;     
 float ReferencePosition_ynew = L1 + TowerHeight - StampHeight;
 float Position_x = 0.0;
 float Position_y = 0.0;
-
+ 
 //set initial conditions for angles, errors and motor values
 float q1 = 0,q2 = PI/2;
 float q1_ref = 0, q2_ref = 0;
@@ -88,56 +88,60 @@
 float q12start = PI/2;
 float q1Encoder = 0, q12Encoder = 0;
 float q12Out = 0;
-
+ 
 float q1_error_prev = 0, q2_error_prev = 0;
 float DerTotalError1 = 0, DerTotalError2 = 0;
 float q1IntError = 0, q2IntError = 0;
 float TotalError1_prev = 0, TotalError2_prev = 0;
 float TotalError1= 0, TotalError2= 0;
-//float TotalErrorMin= 0;
-
+ 
 float motorValue1 = 0.0, motorValue2 = 0.0;
 int counts1 = 0, counts2 = 0;
 int counts1Prev = 0, counts2Prev = 0;
-
+ 
 //set reference angle boundaries
 float q1_refOutNew = 0, q2_refOutNew = 0;
-float q1_refOutMin = 0;                                                     //Physical min angle 0.00 radians
-float q1_refOutMax = 1.37;                                                  //Physical max angle 1.47 radians - 0.1 rad
-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 q1_refOutMin = 0;                 //Physical min angle 0.00 radians
+float q1_refOutMax = 1.37;                  //Physical max angle 1.47 radians - 0.1 rad
+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
+ 
 //set BiQuad filters/filter chains
-
-BiQuad pidf;                                                                //PID Filter
-
-BiQuadChain bcq1R;                                                          //Right EMG filter chain 1: notch filter+highpass
-BiQuadChain bcq2R;                                                          //Right EMG filter chain 2: lowpass
-BiQuad bq1R(9.9110e-01,-1.6036e+00,9.9110e-01,-1.6036e+00,9.8221e-01);      // Notch filter wo=50; bw=wo/35
-BiQuad bq2R(9.1497e-01,-1.8299e+00,9.1497e-01,-1.8227e+00,8.3718e-01);      // High pass Butterworth filter 2nd order, Fc=10;
-BiQuad bq3R(1.3487e-03,2.6974e-03,1.3487e-03,-1.8935e+00,8.9886e-01);       // Low pass Butterworth filter 2nd order, Fc = 8;
-
-BiQuadChain bcq1L;                                                          //Left EMG filter chain 1: notch filter+highpass
-BiQuadChain bcq2L;                                                          //Left EMG filter chain 2: lowpass
-BiQuad bq1L(9.9110e-01,-1.6036e+00,9.9110e-01,-1.6036e+00,9.8221e-01);      // Notch filter wo=50; bw=wo/35
-BiQuad bq2L(9.1497e-01,-1.8299e+00,9.1497e-01,-1.8227e+00,8.3718e-01);      // High pass Butterworth filter 2nd order, Fc=10;
-BiQuad bq3L(1.3487e-03,2.6974e-03,1.3487e-03,-1.8935e+00,8.9886e-01);       // Low pass Butterworth filter 2nd order, Fc = 8;
+BiQuad pidf;                    //PID Filter
+ 
+BiQuadChain bcq1R;              //Right EMG filter chain 1: notch filter+highpass
+BiQuadChain bcq2R;                  //Right EMG filter chain 2: lowpass
+BiQuad bq1R(9.9110e-01,-1.6036e+00,9.9110e-01,-1.6036e+00,9.8221e-01);      
+// Notch filter wo=50; bandwidth=wo/35
+BiQuad bq2R(9.1497e-01,-1.8299e+00,9.1497e-01,-1.8227e+00,8.3718e-01);      
+// High pass Butterworth filter 2nd order, Fc=10;
+BiQuad bq3R(1.3487e-03,2.6974e-03,1.3487e-03,-1.8935e+00,8.9886e-01);       
+// Low pass Butterworth filter 2nd order, Fc = 8;
+ 
+BiQuadChain bcq1L;              //Left EMG filter chain 1: notch filter+highpass
+BiQuadChain bcq2L;                  //Left EMG filter chain 2: lowpass
+BiQuad bq1L(9.9110e-01,-1.6036e+00,9.9110e-01,-1.6036e+00,9.8221e-01);      
+// Notch filter wo=50; bandwidth=wo/35
+BiQuad bq2L(9.1497e-01,-1.8299e+00,9.1497e-01,-1.8227e+00,8.3718e-01);      
+// High pass Butterworth filter 2nd order, Fc=10;
+BiQuad bq3L(1.3487e-03,2.6974e-03,1.3487e-03,-1.8935e+00,8.9886e-01);       
+// Low pass Butterworth filter 2nd order, Fc = 8;
 // In the following: R is used for right arm EMG, L is used for left arm EMG
-
+ 
 //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
+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)
 {   
 //This function reads EMG, filters it and generates a T-switch value which specifies the movement of the robot
-
+ 
     double inLout = emg0.read();
     double inRout = emg1.read();
     
@@ -149,8 +153,11 @@
     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();
+    double biceps_l = (double) envelopeLout * EMGgain; //emg0.read();
+//left biceps filtered EMG signal with a gain
+
+    double biceps_r = (double) envelopeRout * EMGgain;  
+//right biceps filtered EMG signal with a gain
     if (biceps_l > threshold_l && biceps_r > threshold_r){
         //both arms activated: stamp moves down
         Tout = -2;
@@ -168,26 +175,26 @@
         Tout = 0;
         }   
 }
-
-//--------------------------------------------------------------------------------------------------------------------------
-//--------------------------------------------------------------------------------------------------------------------------
-
+ 
+//---------------------------------------------------------------------------------------
+//---------------------------------------------------------------------------------------
+ 
 void GetReferenceKinematics1(float &q1Out, float &q2Out, float &q1_refOut, float &q2_refOut)
 {    
-//This function reads current position from encoder, calculates reference position from T-switch value,
-//converts reference position to reference angles and checks boundaries
+//This function reads current position from encoder, calculates reference position from 
+//T-switch value, converts reference position to reference angles and checks boundaries.
     
     //get joint positions q feedback from encoder
-    float Encoder1Position = counts1/resolution;                            //angular position of encoder in radians
-    float Encoder2Position = -1*counts2/resolution;                         //NEGATIVE due to opposite build up in tower
+    float Encoder1Position = counts1/resolution;    //angular encoder position (rad)
+    float Encoder2Position = -1*counts2/resolution; //NEGATIVE due to opposite build                                    //up in tower
     
     q1Encoder = Encoder1Position*inverse_gear_ratio;
     q12Encoder = Encoder2Position*inverse_gear_ratio;
-    q1Out = q1start + q1Encoder;                                            //angular position of motor axis in radians
-    q12Out = q12start + q12Encoder;                                         //encoder 2 gives sum of q1 and q2!
+    q1Out = q1start + q1Encoder;                    //angular motor axis position (rad)
+    q12Out = q12start + q12Encoder;                 //encoder 2 gives sum of q1 and q2!
     q2Out = q12Out - q1Out;             
-    //float q1deg = q1Out*360/2/PI;                                         //angle in degrees
-    //float q2deg = q2Out*360/2/PI;                                         //angle in degrees
+    //float q1deg = q1Out*360/2/PI;                 //angle in degrees
+    //float q2deg = q2Out*360/2/PI;                 //angle in degrees
     
     //get end effector position with trigonometry
     Position_x = (L1*sin(q1) + L2*sin(q1+q2));
@@ -209,7 +216,7 @@
         ReferencePosition_xnew = ReferencePosition_x + 0.0009;
         ReferencePosition_ynew = y_stampup;
         }
-    else{                                                                   //T==0
+    else{ //T==0
         //no EMG active, no x-movement, y-position restored to non-stamping position
         ReferencePosition_ynew = y_stampup;
         }
@@ -287,17 +294,18 @@
     scope.set(4, T);
     scope.send();
 }    
-
-//--------------------------------------------------------------------------------------------------------------------------
-//--------------------------------------------------------------------------------------------------------------------------
-
+ 
+//---------------------------------------------------------------------------------------
+//---------------------------------------------------------------------------------------
+ 
 void FeedbackControl1(float q1_ref, float q2_ref, float q1, float q2, float &motorValue1Out, float &motorValue2Out)
 {
-//This function calculates the error between angles and refernce angles, and provides motor values via a PIDF controller    
+//This function calculates the error between angles and refernce angles, and provides
+//motor values via a PIDF controller.
     
     //calculate error values
-    float q1_error = q1_ref - q1;                                           // proportional angular error in radians
-    float q2_error = q2_ref - q2;                                           // proportional angular error in radians
+    float q1_error = q1_ref - q1;               // proportional angular error in radians
+    float q2_error = q2_ref - q2;               // proportional angular error in radians
     
     //PIDF total error output
     float TotalError1 = pidf.step(q1_error);
@@ -313,15 +321,15 @@
     TotalError1_prev = TotalError1;
     TotalError2_prev = TotalError2;
 }
-
-//--------------------------------------------------------------------------------------------------------------------------
-//--------------------------------------------------------------------------------------------------------------------------
-
+ 
+//---------------------------------------------------------------------------------------
+//---------------------------------------------------------------------------------------
+ 
 void SetMotor1(float motorValue1, float motorValue2)
 {
 //This function sets the PWM and direction bits for the motors. 
 //motorValues outside range are truncated to within range.
-
+ 
     //control motor 1
     if (motorValue1 >=0){ 
         //clockwise rotation
@@ -332,7 +340,7 @@
         motor1DirectionPin=ccw;
         }
     if (fabs(motorValue1)>MotorMaxSpeed){
-        motor1MagnitudePin = MotorMaxSpeed;                                 //motor values truncated
+        motor1MagnitudePin = MotorMaxSpeed;             //motor values truncated
         }
     else{
         motor1MagnitudePin = fabs(motorValue1);
@@ -341,13 +349,11 @@
     //control motor 2
     if (motorValue2 >=0){  
         //counterclockwise rotation due to inverse buildup in tower
-        motor2DirectionPin=cw;                                             //action is ccw, due to faulty motor2DirectionPin (inverted)
+        motor2DirectionPin=cw;                  //action is ccw, due to faulty                                  motor2DirectionPin (inverted)
         }
     else{    
         //clockwise rotation due to inverse buildup in tower
-        motor2DirectionPin=ccw;                                            //action is cw, due to faulty motor2DirectionPin (inverted)
-        //led1=0;
-        //led2=1;
+        motor2DirectionPin=ccw;                         //action is cw, due to faulty                                   motor2DirectionPin (inverted)
         }
     if (fabs(motorValue2)>MotorMaxSpeed){
         motor2MagnitudePin = MotorMaxSpeed;
@@ -356,10 +362,10 @@
         motor2MagnitudePin = fabs(motorValue2);
         }
 }
-
-//--------------------------------------------------------------------------------------------------------------------------
-//--------------------------------------------------------------------------------------------------------------------------
-
+ 
+//---------------------------------------------------------------------------------------
+//---------------------------------------------------------------------------------------
+ 
 void MeasureAndControl()
 {
 // This function measures the EMG of both arms, calculates via inverse kinematics
@@ -370,10 +376,10 @@
     FeedbackControl1( q1_ref, q2_ref, q1, q2, motorValue1, motorValue2);
     SetMotor1(motorValue1, motorValue2);
 }
-
-//--------------------------------------------------------------------------------------------------------------------------
-//--------------------------------------------------------------------------------------------------------------------------
-
+ 
+//---------------------------------------------------------------------------------------
+//---------------------------------------------------------------------------------------
+ 
 int main()
 {
  //pc.baud(115200);
@@ -382,17 +388,18 @@
  ledBlue=1; 
  ledRed=0;
 
- bcq1R.add(&bq1R).add(&bq2R);                                               //set BiQuad chains
+ //set BiQuad chains
+ bcq1R.add(&bq1R).add(&bq2R);                       
  bcq2R.add(&bq3R);
  bcq1L.add(&bq1L).add(&bq2L);
  bcq2L.add(&bq3L);
- pidf.PIDF( Kp, Ki, Kd, N, t_sample );                                      //set PID filter
+ pidf.PIDF( Kp, Ki, Kd, N, t_sample );                  //set PID filter
  
  //start up encoders
- counts1 = Encoder1.getPulses();                                            //gives position of encoder in counts 
- counts2 = Encoder2.getPulses();                                            //gives position of encoder in counts
- wait(20.0);                                                                //time to sart up HIDScope and EMG
- MeasureTicker.attach(&MeasureTicker_act, 0.002);                           //initialize MeasureTicker, 500 Hz
+ counts1 = Encoder1.getPulses();                        //gives position of encoder in counts 
+ counts2 = Encoder2.getPulses();                        //gives position of encoder in counts
+ wait(20.0);                                            //time to sart up HIDScope and EMG
+ MeasureTicker.attach(&MeasureTicker_act, 0.002);       //initialize MeasureTicker, 500 Hz
  
  while(1)
     {
@@ -400,9 +407,9 @@
             MeasureTicker_go=false;
             ledGrn = 1;
             ledBlue = 0;
-            MeasureAndControl();                                            //execute whole MeasureAndControl function
-            counts1 = Encoder1.getPulses();                                 //get encoder counts again
-            counts2 = Encoder2.getPulses();                                 //get encoder counts again
+            MeasureAndControl();                    //execute MeasureAndControl
+            counts1 = Encoder1.getPulses();         //get encoder counts again
+            counts2 = Encoder2.getPulses();             //get encoder counts again
             ledBlue = 1;
             ledGrn = 0;
             }