Diff: main.cpp

Revision:

33:b09608fa69e9

Parent:

29:caf3dd699617

Child:

34:96bcbddc7d2d

--- a/main.cpp	Fri Oct 28 09:17:12 2016 +0000
+++ b/main.cpp	Tue Nov 01 12:35:16 2016 +0000
@@ -38,10 +38,14 @@
 DigitalIn button1(D3);
 DigitalIn button2(D9);
 
+DigitalOut ledgrn(LED_GREEN);
+DigitalOut ledred(LED_RED);
+DigitalOut ledblue(LED_BLUE);
+
 //library settings
 Serial pc(USBTX,USBRX);
 Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
-HIDScope    scope(6);
+//HIDScope    scope(6);
 
 //initial values
 float dx;
@@ -151,11 +155,13 @@
     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();
@@ -204,7 +210,7 @@
         ReferencePosition_xnew = ReferencePosition_x;
         ReferencePosition_ynew = ReferencePosition_y;
         }
-        }
+      //  }
     
     float PointPositionArm2_x = ReferencePosition_x;
     float PointPositionArm2_y = ReferencePosition_y - TowerHeight + StampHeight;
@@ -248,20 +254,11 @@
     //q1Out = q1Out + q1_dotOut;  //in radians
     //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.baud(115200);
+    //pc.printf("q1: %f   ", q1Out);
+    //pc.printf("q1ref: %f    ", q1_refOut);
+    //pc.printf("q2: %f   ", q2Out);
+    //pc.printf("q2ref: %f    ", q2_refOut);
     
     /*
     pc.printf("dx:          %f \r\n", dx);
@@ -287,17 +284,17 @@
     // 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 = 3; //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;
-    float Ki = 0.1; //potMeter2.read();
+    float Ki = 0.04;  //0.01*Kp; //potMeter2.read();
     
     float q1DerivativeError = (q1_error - q1_error_prev)/t_sample;  // derivative of error in radians
     float q2DerivativeError = (q2_error_prev + q2_error)/t_sample;  // derivative of error in radians
     //float maxKd = 0.2;
-    float Kd = 0.0; //potMeter2.read();
+    float Kd = 0.0;  //0.04*Kp; //potMeter2.read();
     
     //scope.set(0,referencePosition1);
     //scope.set(1,Position1);
@@ -307,19 +304,20 @@
     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 (fabs(TotalError1) < TotalErrorMin) {
         TotalError1=0;
         }
     else {
         TotalError1=TotalError1;
         }
-    if (TotalError2 < TotalErrorMin) {
+    if (fabs(TotalError2) < TotalErrorMin) {
         TotalError2=0;
         }
     else {
         TotalError2=TotalError2;
         }
-    
+    */
     /*
     DerTotalError1 = (TotalError1 - TotalError1_prev)/t_sample;
     DerTotalError1 = (TotalError1 - TotalError1_prev)/t_sample;
@@ -330,10 +328,7 @@
     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);
@@ -341,28 +336,24 @@
     scope.set(4,motorValue1Out);
     scope.set(5,motorValue2Out);
     scope.send();
+    */
+    /*
+    pc.printf("E1: %f  ", q1_error);
+    pc.printf("IE1: %f  ", q1IntError);
+    pc.printf("DE1: %f  ", q2DerivativeError);
+    pc.printf("E2: %f  ", q2_error);
+    pc.printf("IE2: %f  ", q2IntError);
+    pc.printf("DE2: %f  ", q2DerivativeError);
     
-    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("TE1: %f  ", TotalError1);
+    pc.printf("TE2: %f  ", TotalError2);
+    pc.printf("M1: %f   ", motorValue1Out);
+    pc.printf("M2: %f   \r\n", motorValue2Out);
+    */  
     q1_error_prev = q1_error;
     q2_error_prev = q2_error;
     TotalError1_prev = TotalError1;
-    TotalError1_prev = TotalError1;
-    */
-     
-    /*
-    scope.set(0,q1);
-    scope.set(1,q2);
-    scope.set(2,biceps_l);
-    scope.set(3,biceps_r);
-    scope.send();
-    */ 
+    TotalError2_prev = TotalError2;
 }
 
 void SetMotor1(float motorValue1, float motorValue2)
@@ -408,8 +399,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()
@@ -445,20 +436,24 @@
  //Initialize
  //int led1val = led1.read();
  //int led2val = led2.read();
-  pc.baud(115200);
- pc.printf("Test putty IK");
- MeasureTicker.attach(&MeasureTicker_act, 0.1f); 
+ //pc.baud(115200);
+ //pc.printf("Test putty IK");
+ MeasureTicker.attach(&MeasureTicker_act, 0.01f); 
  bqc.add(&bq1).add(&bq2);
  
  while(1)
     {
         if (MeasureTicker_go){
             MeasureTicker_go=false;
+            ledgrn = 1;
+            ledblue = 0;
             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  ", counts1);
+            //pc.printf("counts2: %i  \r\n", counts2);
+            ledblue = 1;
+            ledgrn = 0;
             }
 /*
         if (BiQuadTicker_go){