Diff: main.cpp

Revision:

7:de221f894d3b

Parent:

6:405ec2bba6d0

Child:

8:fa268d163bbd

--- a/main.cpp	Thu Nov 01 18:50:26 2018 +0000
+++ b/main.cpp	Fri Nov 02 08:15:25 2018 +0000
@@ -21,16 +21,17 @@
 DigitalOut Motor2Direction(D4);   
 //EMG
 
+/*
 AnalogIn    a0(A0);
 AnalogIn    a1(A1);
 AnalogIn    a2(A2);
 AnalogIn    a3(A3);
-
+*/
 
-/*AnalogIn Ppot(A0);
+AnalogIn Ppot(A0);
 AnalogIn Ipot(A1);
 AnalogIn Dpot(A2);
-*/
+
 
 MODSERIAL pc(USBTX, USBRX);
 
@@ -65,7 +66,9 @@
 BiQuad LP3(1,2,1,1.911197067426073,0.914975834801434);
 BiQuad LP4(1,2,1,1.911197067426073,0.914975834801434);
 
-
+// lowpass derivative
+BiQuad LPD1(1,2,1,-1.734725768809275,0.766006600943264);
+BiQuad LPD2(1,2,1,-1.734725768809275,0.766006600943264);
 
 ///Variables
 
@@ -123,7 +126,7 @@
 
 //States
 enum states {WaitModusState, EMGCalibrationState, NormalOperatingModusState, DemoModusState};
-states State = WaitModusState;
+states State = DemoModusState;
 
 
 //Calibration Time
@@ -135,12 +138,12 @@
 
 void ReadEMG()
 {
-
+/*
     EMGdata1 = a0.read(); // store values in the scope
     EMGdata2 = a1.read();
     EMGdata3 = a2.read();
     EMGdata4 = a3.read();
-
+*/
 }
 
 // EMG High pass filters
@@ -356,16 +359,20 @@
      
         //D-Control
         //First smoothen the error signal
+        
         double MovAvq1_1 = 0.0;
         double MovAvq1_2 = 0.0;
+        
         for (int i=0; i<=n-2; i++){   // Creates two mov. av. with one element shifted
             MovAvq1_1 = MovAvq1_1 + PrevErrorq1[i]/((double) (n-1));
             MovAvq1_2 = MovAvq1_2 + PrevErrorq1[i+1]/((double)(n-1));
         }
-        DerivativeErrorq1 = (MovAvq1_2 - MovAvq1_1)/TickerPeriod;    
+        DerivativeErrorq1 = (MovAvq1_2 - MovAvq1_1)/TickerPeriod;
         double D_error1 = GainD1 * DerivativeErrorq1;
+        
         // Derivative error sum over all time?   
-     
+        
+        
     PIDerrorq1 = P_error1 + I_error1 + D_error1;
     
     // PID control motor 2
@@ -378,6 +385,7 @@
      
         //D-Control
         //First smoothen the error signal
+        
         double MovAvq2_1 = 0.0;
         double MovAvq2_2 = 0.0;
         for (int i=0; i<=n-2; i++){   // Creates two mov. av. with one element shifted
@@ -386,6 +394,8 @@
         }
         DerivativeErrorq2 = (MovAvq2_2 - MovAvq2_1)/TickerPeriod;    
         double D_error2 = GainD2 * DerivativeErrorq2;
+        
+        
         // Derivative error sum over all time?   
      
     PIDerrorq2 = P_error2 + I_error2 + D_error2;
@@ -484,19 +494,10 @@
     }
 }
 void TestPath()
-{
-   /* GainP1 = Ppot.read()*60.0;
-    GainI1 = Ipot.read()*20.0;
-    GainD1 = Dpot.read();
-    GainP2 = Ppot.read()*30.0;
-    GainI2 = Ipot.read()*20.0;
-    GainD2 = Dpot.read();
-    
-    pc.printf("GainP = %1f, GainI = %1f, GainD = %1f", GainP1, GainI1, GainD1);
-    // Also think about how to move from whatever position to (40,5)
-    if (DemoStage == 0)    //From (40,40) to (40,-5)
+{ 
+   if (DemoStage == 0)    //From (40,40) to (40,-5)
     {
-        if (yRef >0)
+        if (yRef > 20.0)
         {
             yRef = yRef - vyMax * TickerPeriod;
         }
@@ -505,22 +506,25 @@
             DemoStage = 1;
         }
     }
+   
     else if (DemoStage == 1)
     {
-        if (yRef < 30)      //From (65,-5) to (65, 10)
+        if (yRef < 35.0)      //From (65,-5) to (65, 10)
         {
-            yRef = yRef + vyMax * TickerPeriod;
+            yRef = yRef + TickerPeriod;
         }
         else
         {
             DemoStage = 0;
         }
     }
-    */
+
 }
 
 
 
+
+
 void CalibrationButton()
 {
     ledred = 1;
@@ -581,12 +585,12 @@
 void DemoModus()   // The ticker should call this function (in the switch statement)
 {    
 
-    //GainP1 = pot3.read() * 30;
-    //GainI1 = pot2.read() * 10;
-    //GainD1 = pot1.read() ;
-    //GainP2 = pot3.read() * 10;
-    //GainI2 = pot2.read() * 10;
-    //GainD2 = pot1.read() ;
+   GainP1 = 40.0;//Ppot.read()*100.0;
+    GainI1 = 17.0;//Ipot.read()*20.0;
+    GainD1 = 2.0;//Dpot.read()*20.0;
+    GainP2 = Ppot.read()*100.0;
+    GainI2 = Ipot.read()*20.0;
+    GainD2 = Dpot.read()*20.0;
      
     //DemonstrationPath();
     TestPath();
@@ -604,7 +608,7 @@
     count++;
     if (count ==400)
     {
-    pc.printf("GainP = %lf, GainI = %lf, GainD = %lf, q1Pos = %lf, q2Pos = %lf, q1Ref = %lf, q2Ref = %lf,   xRef = %lf, yRef = %lf \n\r", GainP1, GainI1, GainD1, q1Pos, q2Pos, q1Ref, q2Ref, xRef, yRef); 
+    pc.printf("GainP = %lf, GainI = %lf, GainD = %lf, q1Pos = %lf, q2Pos = %lf, q1Ref = %lf, q2Ref = %lf,   xRef = %lf, yRef = %lf \n\r", GainP2, GainI2, GainD2, q1Pos, q2Pos, q1Ref, q2Ref, xRef, yRef); 
     count = 0;
     }
 }
@@ -674,7 +678,7 @@
                 break;
         case DemoModusState:
              DemoModus();    
-             pc.printf("Demo \n\r");  
+             //pc.printf("Demo \n\r");  
                 break;
         default :        
     }
@@ -705,14 +709,14 @@
         bqc4.add( &HP_emg4 ).add( &Notch_emg4 );//.add( &LP4);
         
         //Initialize array errors to 0
-        for (int i = 0; i <= 9; i++){
-             PrevErrorq2[i] = 0;
+        for (int i = 0; i <= 99; i++){
+             PrevErrorq1[i] = 0;
              PrevErrorq2[i] = 0;
              }
              
-        int frequency_pwm = 16700; //16.7 kHz PWM
-        Motor1PWM.period(1.0/frequency_pwm); // T = 1/f
-        Motor2PWM.period(1.0/frequency_pwm); // T = 1/f     
+        double frequency_pwm = 16700.0; //16.7 kHz PWM
+        Motor1PWM.period_us(1.0/frequency_pwm); // T = 1/f
+        Motor2PWM.period_us(1.0/frequency_pwm); // T = 1/f     
         
        //Emg Calibratie button
         //ButtonCal.fall(&CalibrationButton);