Diff: main.cpp

Revision:

31:880ebd2e8145

Parent:

30:676ecd59467a

--- a/main.cpp	Fri Oct 28 09:31:08 2016 +0000
+++ b/main.cpp	Fri Oct 28 14:21:46 2016 +0000
@@ -16,9 +16,8 @@
 - Set angle and length boundaries!!
 - Set robot constants (lengths etc.)
 - Set EMGgain and thresholds
-- Add tower height to ReferencePosition_y and Position_y AND inverse kinematics calculation!
-- Add (lower) boundaries to TotalErrors
-- MotorGain could change due to arm weight!!
+//- Add tower height to ReferencePosition_y and Position_y AND inverse kinematics calculation!
+- MotorGain could change due to arm weight!
 - Arms should be placed manually into reference position.
 */
 
@@ -41,7 +40,7 @@
 //library settings
 Serial pc(USBTX,USBRX);
 Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
-HIDScope    scope(6);
+//HIDScope    scope(6);
 
 //initial values
 float dx;
@@ -79,6 +78,10 @@
 
 float q1_error_prev = 0;
 float q2_error_prev = 0;
+float TotalError1 = 0;
+float TotalError2 = 0;
+float TotalErrorMin= 0;
+
 float DerTotalError1 = 0;
 float DerTotalError2 = 0;
 float q1IntError = 0;
@@ -93,14 +96,16 @@
 int counts1Prev = 0;
 int counts2Prev = 0;
 
+int errorSwitch = 0;
 
 //set constant or variable values (VALUES HAVE TO BE EDITED)
-float EMGgain = 1.0;
-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 (motor value of 1)
-float MotorSpeedGain = 5.0; //to give the motor higher speeds
-float t_sample = 0.01; //seconds
+const float EMGgain = 1.0;
+//float dy_stampdown = L1 + TowerHeight - StampHeight; //0.05; //5 cm movement downward to stamp
+const float MotorGain = 8.4; // rad/s for PWM, is max motor speed (motor value of 1)
+const float MotorSpeedGain = 30; //MotorValue is divided by MotorSpeedGain to give the motor higher speeds
+const float t_sample = 0.01; //seconds
 const float maxStampDistance = 1.5;
+const float EMGThreshold = 0.0;
 
 float q1_refOutNew = 0;
 float q1_refOutMin = 0;         //WRONG values
@@ -108,9 +113,6 @@
 float q2_refOutNew = 0;
 float q2_refOutMin = 0;         //WRONG values
 float q2_refOutMax = PI;        //WRONG values
-float TotalError1= 0;
-float TotalError2= 0;
-float TotalErrorMin= 0;
  
 //set BiQuad
 BiQuadChain bqc;
@@ -156,19 +158,26 @@
      wait(1.0); 
      ReferencePosition_ynew = L1 + TowerHeight - StampHeight;      //Reset vertical position after stamping
       }
-    else{      
+    else{                                                           //move as reaction on EMG signals
     //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG  
     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){
+    if (biceps_1 > EMGThreshold || biceps_r > EMGThreshold){    //if new movement is initiated
+        errorSwitch = 0;
+        }
+    else {
+        errorSwitch = errorSwitch;
+        }
+    if (biceps_l > EMGThreshold && biceps_r > EMGThreshold){
         //both arms activated: stamp moves down
         //led1 = 1;
         //led2 = 1;
+        float dy_stampdown = Position_y; //Position_y movement downward to stamp
         ReferencePosition_xnew = ReferencePosition_x;
         ReferencePosition_ynew = ReferencePosition_y - dy_stampdown;   //into stamping vertical position ~the stamp down action
         
         }
-    else if (biceps_l > 0 && biceps_r <= 0){
+    else if (biceps_l > EMGThreshold && biceps_r <= EMGThreshold){
         //arm 1 activated, move left
         //led1 = 1;
         //led2 = 0;
@@ -184,7 +193,7 @@
         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)));
         */
         }
-    else if (biceps_l <= 0 && biceps_r > 0){
+    else if (biceps_l <= EMGThreshold && biceps_r > EMGThreshold){
         //arm 1 activated, move right
         //led1 = 0;
         //led2 = 1;
@@ -250,18 +259,19 @@
     //q2Out = q2Out + q2_dotOut;
     
     pc.baud(115200);
-    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("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 ", q1Out);
+    //pc.printf("Reference_x:     %f m \r\n", ReferencePosition_x);
+    //pc.printf(" q1ref: %f rad    ", q1_refOut);
+    //pc.printf("Position_y:      %f m\r\n", Position_y);
+    //pc.printf(" q2Out: %f rad ", q2Out);
+    //pc.printf("Reference_y:     %f m \r\n", ReferencePosition_y);
+    //pc.printf(" q2ref: %f rad    ", q2_refOut);
+    //pc.printf("alpha:           %f rad \r\n", alpha);
+    //pc.printf("beta:            %f rad\r\n", beta);
     
     
     /*
@@ -288,8 +298,8 @@
     // linear feedback control
     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 Kp = 1.0; //potMeter2.read();
+    
     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;
@@ -300,22 +310,20 @@
     //float maxKd = 0.2;
     float Kd = 0.0; //potMeter2.read();
     
-    //scope.set(0,referencePosition1);
-    //scope.set(1,Position1);
-    //scope.set(2,Ki);
-    //scope.send();
-    
     TotalError1 = q1_error * Kp + q1IntError * Ki + q1DerivativeError * Kd;         //total controller output in radians = motor input
     TotalError2 = q2_error * Kp + q2IntError * Ki + q2DerivativeError * Kd;         //total controller output in radians = motor input
     
-    if (TotalError1 < TotalErrorMin) {
+    
+    if (TotalError1 < 0 && errorSwitch <= 0) {
         TotalError1=0;
+        errorSwitch = -1;
         }
     else {
         TotalError1=TotalError1;
         }
-    if (TotalError2 < TotalErrorMin) {
-        TotalError2=0;
+    if (TotalError1 >= 0 && errorSwitch >= 0) {
+        TotalError1=TotalError1;
+        errorSwitch = 1;
         }
     else {
         TotalError2=TotalError2;
@@ -324,20 +332,26 @@
     /*
     DerTotalError1 = (TotalError1 - TotalError1_prev)/t_sample;
     DerTotalError1 = (TotalError1 - TotalError1_prev)/t_sample;
-    motorValue1Out = DerTotalError1/MotorGain;       
-    motorValue2Out = DerTotalError2/MotorGain;       
+    float motorValue1Small = DerTotalError1/MotorGain;       
+    float motorValue2Small = DerTotalError2/MotorGain;       
     */
     
+    float PseudoDerError1 = TotalError1/t_sample;
+    float PseudoDerError2 = TotalError2/t_sample;
+    float motorValue1Small = PseudoDerError1/MotorGain;     //~motorValue1Small = TotalError1*11.90  
+    float motorValue2Small = PseudoDerError2/MotorGain;       
+    /*
     float motorValue1Small = TotalError1/MotorGain;       
     float motorValue2Small = TotalError2/MotorGain;       
-    
-    motorValue1Out = MotorSpeedGain*motorValue1Small;
-    motorValue2Out = MotorSpeedGain*motorValue2Small;
+    */
+      
+    motorValue1Out = motorValue1Small/MotorSpeedGain;
+    motorValue2Out = motorValue2Small/MotorSpeedGain;
     
     TotalError1_prev = TotalError1;
     TotalError2_prev = TotalError2;
     
-    
+    /*
     scope.set(0,q1_ref);
     scope.set(1,q1);
     scope.set(2,q2_ref);
@@ -345,14 +359,17 @@
     scope.set(4,motorValue1Out);
     scope.set(5,motorValue2Out);
     scope.send();
-    
-    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);
-    
+    */
+    //pc.printf(" error1: %f  ", q1_error );
+    //pc.printf("IntError1: %f \r\n", q1IntError);
+    //pc.printf("DerError1: %f \r\n", q1DerivativeError);
+    //pc.printf(" error2: %f  ", q2_error);
+    //pc.printf("IntError2: %f \r\n", q2IntError);
+    //pc.printf("DerError2: %f \r\n", q2DerivativeError);
+    pc.printf("E1 %f ", TotalError1);
+    pc.printf("E2 %f ", TotalError2);
+    pc.printf("M1 %f ", motorValue1Out);
+    pc.printf("M2 %f\r\n", motorValue2Out);
     /*    
     q1_error_prev = q1_error;
     q2_error_prev = q2_error;
@@ -412,8 +429,8 @@
         }
         float ReadMagn1 = motor1MagnitudePin.read();
         float ReadMagn2 = motor2MagnitudePin.read();
-        pc.printf("motor1Magnitude: %f \r\n", ReadMagn1);
-        pc.printf("motor2Magnitude: %f \r\n", ReadMagn2);
+        //pc.printf("motor1Magnitude: %f \r\n", ReadMagn1);
+        //pc.printf("motor2Magnitude: %f \r\n", ReadMagn2);
 }
 
 void MeasureAndControl()
@@ -450,8 +467,9 @@
  //int led1val = led1.read();
  //int led2val = led2.read();
   pc.baud(115200);
- pc.printf("Test putty IK");
- MeasureTicker.attach(&MeasureTicker_act, 0.1f); 
+ //pc.printf("Test putty IK");
+ float TickerFreq = 1/t_sample; //0.01f
+ MeasureTicker.attach(&MeasureTicker_act, 0.01f); 
  bqc.add(&bq1).add(&bq2);
  
  while(1)
@@ -461,8 +479,8 @@
             MeasureAndControl();
             counts1 = Encoder1.getPulses();           // gives position of encoder 
             counts2 = Encoder2.getPulses();           // gives position of encoder 
-            pc.printf("counts1:         %i \r\n", counts1);
-            pc.printf("counts2:         %i \r\n", counts2);
+            //pc.printf("counts1:         %i \r\n", counts1);
+            //pc.printf("counts2:         %i \r\n", counts2);
             }
 /*
         if (BiQuadTicker_go){