Diff: main.cpp

Revision:

29:caf3dd699617

Parent:

28:6d8c6fe79394

Child:

30:676ecd59467a

Child:

33:b09608fa69e9

--- a/main.cpp	Thu Oct 27 10:35:53 2016 +0000
+++ b/main.cpp	Fri Oct 28 09:17:12 2016 +0000
@@ -41,7 +41,7 @@
 //library settings
 Serial pc(USBTX,USBRX);
 Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
-//HIDScope    scope(4);
+HIDScope    scope(6);
 
 //initial values
 float dx;
@@ -83,6 +83,8 @@
 float DerTotalError2 = 0;
 float q1IntError = 0;
 float q2IntError = 0;
+float TotalError1_prev = 0;
+float TotalError2_prev = 0;
 
 float motorValue1 = 0.0;
 float motorValue2 = 0.0;
@@ -148,7 +150,12 @@
     Position_x = (L1*sin(q1) + L2*sin(q1+q2));
     Position_y = (L1*cos(q1) + L2*cos(q1+q2)) + TowerHeight - StampHeight;  
     //float PositionVector = sqrt(pow(Position_x,2)+pow(Position_y,2));
-      
+    
+    if (Position_y < (0.5*TowerHeight)){
+     wait(1.0); 
+     ReferencePosition_ynew = L1 + TowerHeight - StampHeight;      //Reset vertical position after stamping
+      }
+    else{      
     //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();
@@ -159,14 +166,6 @@
         ReferencePosition_xnew = ReferencePosition_x;
         ReferencePosition_ynew = ReferencePosition_y - dy_stampdown;   //into stamping vertical position ~the stamp down action
         
-        /*
-         wait(1);   //lift stamp after stamping
-        ReferencePosition_y = ReferencePosition_y - dy_stampdown;   
-        PositionError_y = ReferencePosition_y - Position_y;        //Position error in dx,dy
-        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)));
-        */
         }
     else if (biceps_l > 0 && biceps_r <= 0){
         //arm 1 activated, move left
@@ -205,6 +204,7 @@
         ReferencePosition_xnew = ReferencePosition_x;
         ReferencePosition_ynew = ReferencePosition_y;
         }
+        }
     
     float PointPositionArm2_x = ReferencePosition_x;
     float PointPositionArm2_y = ReferencePosition_y - TowerHeight + StampHeight;
@@ -278,6 +278,7 @@
     pc.printf("Encoder2:    %f \r\n", Encoder2Position);
     pc.printf("Motor2:      %f \r\n", q2Out);
     */
+    
     }
     
 
@@ -329,6 +330,18 @@
     motorValue1Out = TotalError1/MotorGain;       
     motorValue2Out = TotalError2/MotorGain;       
     
+    TotalError1_prev = TotalError1;
+    TotalError2_prev = TotalError2;
+    
+    
+    scope.set(0,q1_ref);
+    scope.set(1,q1);
+    scope.set(2,q2_ref);
+    scope.set(3,q2);
+    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);
@@ -369,8 +382,13 @@
         //led1=0;
         //led2=1;
         }
-    if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
-        else motor1MagnitudePin = fabs(motorValue1);  //fabs(motorValue1);
+    if (fabs(motorValue1)>1){
+        motor1MagnitudePin = 1;
+        }
+    else{
+        motor1MagnitudePin = fabs(motorValue1);  //fabs(motorValue1);
+        }
+    
     //control motor 2
     if (motorValue2 >=0)  //clockwise rotation
         {motor2DirectionPin=ccw; //action is cw, due to faulty motor2DirectionPin (inverted)
@@ -382,8 +400,12 @@
         //led1=0;
         //led2=1;
         }
-    if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
-        else motor2MagnitudePin = fabs(motorValue2);  //fabs(motorValue1);
+    if (fabs(motorValue2)>1){
+        motor2MagnitudePin = 1;
+        }
+    else{
+        motor2MagnitudePin = fabs(motorValue2);
+        }
         float ReadMagn1 = motor1MagnitudePin.read();
         float ReadMagn2 = motor2MagnitudePin.read();
         pc.printf("motor1Magnitude: %f \r\n", ReadMagn1);