Diff: main.cpp

Revision:

6:405ec2bba6d0

Parent:

5:8e326d07f125

Child:

7:de221f894d3b

--- a/main.cpp	Wed Oct 31 16:08:45 2018 +0000
+++ b/main.cpp	Thu Nov 01 18:50:26 2018 +0000
@@ -20,15 +20,22 @@
 FastPWM Motor2PWM(D5);            
 DigitalOut Motor2Direction(D4);   
 //EMG
+
 AnalogIn    a0(A0);
 AnalogIn    a1(A1);
 AnalogIn    a2(A2);
 AnalogIn    a3(A3);
 
+
+/*AnalogIn Ppot(A0);
+AnalogIn Ipot(A1);
+AnalogIn Dpot(A2);
+*/
+
 MODSERIAL pc(USBTX, USBRX);
 
 //HIDSCOPE          //Oppassen waar we HIDSCOPE aanroepen want nu voor meerdere dingen
-HIDScope    scope(4);
+HIDScope    scope(6);
 Ticker      scopeTimer;
 
 Ticker      EMGTicker;
@@ -41,16 +48,24 @@
 BiQuadChain bqc4;
 
     //High pass filters 20 Hz
-BiQuad HP_emg1(1,-2,1,-1.142980502539901,0.412801598096189);
-BiQuad HP_emg2(1,-2,1,-1.142980502539901,0.412801598096189); 
-BiQuad HP_emg3(1,-2,1,-1.142980502539901,0.412801598096189); 
-BiQuad HP_emg4(1,-2,1,-1.142980502539901,0.412801598096189);
+BiQuad HP_emg1(1,-2,1,-1.647459981076977,0.700896781188403);
+BiQuad HP_emg2(1,-2,1,-1.647459981076977,0.700896781188403); 
+BiQuad HP_emg3(1,-2,1,-1.647459981076977,0.700896781188403); 
+BiQuad HP_emg4(1,-2,1,-1.647459981076977,0.700896781188403);
 
     //Notch filters 50 Hz
-BiQuad Notch_emg1(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492); 
-BiQuad Notch_emg2(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492); 
-BiQuad Notch_emg3(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492); 
-BiQuad Notch_emg4(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492);
+BiQuad Notch_emg1(1,-1.619184463611223,1,-1.560333397539390,0.927307768331003);
+BiQuad Notch_emg2(1,-1.619184463611223,1,-1.560333397539390,0.927307768331003);
+BiQuad Notch_emg3(1,-1.619184463611223,1,-1.560333397539390,0.927307768331003); 
+BiQuad Notch_emg4(1,-1.619184463611223,1,-1.560333397539390,0.927307768331003);
+
+    //LP 98
+BiQuad LP1(1,2,1,1.911197067426073,0.914975834801434);
+BiQuad LP2(1,2,1,1.911197067426073,0.914975834801434);
+BiQuad LP3(1,2,1,1.911197067426073,0.914975834801434);
+BiQuad LP4(1,2,1,1.911197067426073,0.914975834801434);
+
+
 
 ///Variables
 
@@ -77,7 +92,7 @@
 double GainP2 = 3.0;
 double GainI2 = 0.0;
 double GainD2 = 0.235;
-double TickerPeriod = 1.0/400.0;
+double TickerPeriod = 1.0/500.0;
 // Global variables EMG
 double EMGdata1;
 double EMGdata2;
@@ -102,8 +117,8 @@
 int countstep = 0;
 
 //Demo variabelen
-double vxMax = 4.0;
-double vyMax = 4.0;
+double vxMax = 1.0;
+double vyMax = 1.0;
 int DemoStage = 0;
 
 //States
@@ -120,10 +135,12 @@
 
 void ReadEMG()
 {
+
     EMGdata1 = a0.read(); // store values in the scope
     EMGdata2 = a1.read();
     EMGdata3 = a2.read();
     EMGdata4 = a3.read();
+
 }
 
 // EMG High pass filters
@@ -250,17 +267,19 @@
     }
         
 
-    scope.set(0, unit_vX);
-    scope.set(1, unit_vY);
-    scope.set(2, EMG_filt1);
-    scope.set(3, EMG_filt2);
+    scope.set(0, EMG_filt1);
+    scope.set(1, EMG_filt2);
+    scope.set(2, unit_vX);
+    scope.set(3, EMG_filt3);
+    scope.set(4, EMG_filt4);
+    scope.set(5, unit_vY);
     
 
     count++;
 
     if (count == 100) 
     {
-        //pc.printf("EMG filt 1 = %lf \t EMG filt 2 = %lf \t EMG filt 3 = %lf \t EMG filt 4 = %lf \n\r", EMG_filt1, EMG_filt2, EMG_filt3, EMG_filt4);
+        pc.printf("xRef = %1f, yRef = %1f, q2Ref = %1f, q1Ref = %1f \n\r", xRef, yRef, q2Ref, q1Ref);
         count = 0;
     }
 }
@@ -286,7 +305,7 @@
 void InverseKinematics ()// Kinematics function, takes imput between 1 and -1
 {
     
-    double V_max = 1.0; //Maximum velocity in either direction
+    double V_max = 2.5; //Maximum velocity in either direction
     //     CM/s
     
     double deltaX = TickerPeriod*V_max*unit_vX; // imput to delta
@@ -464,6 +483,41 @@
         }
     }
 }
+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 (yRef >0)
+        {
+            yRef = yRef - vyMax * TickerPeriod;
+        }
+        else
+        {
+            DemoStage = 1;
+        }
+    }
+    else if (DemoStage == 1)
+    {
+        if (yRef < 30)      //From (65,-5) to (65, 10)
+        {
+            yRef = yRef + vyMax * TickerPeriod;
+        }
+        else
+        {
+            DemoStage = 0;
+        }
+    }
+    */
+}
 
 
 
@@ -534,7 +588,8 @@
     //GainI2 = pot2.read() * 10;
     //GainD2 = pot1.read() ;
      
-    DemonstrationPath();
+    //DemonstrationPath();
+    TestPath();
     inverse();
     ReadCurrentPosition();
     UpdateError();
@@ -579,7 +634,7 @@
      counter++;
      if (counter == 400) // print 1x per seconde waardes.
     {
-        pc.printf("xRef = %lf, q1Pos = %lf, q1Ref = %1f \n\r", xRef, q1Pos, q1Ref);
+        //pc.printf("xRef = %lf, q1Pos = %lf, q1Ref = %1f \n\r", xRef, q1Pos, q1Ref);
         counter = 0;   
     }
      if (countstep == 4000)
@@ -636,18 +691,18 @@
         GainI1 = pot2.read() * 10;
         GainD1 = pot1.read();
         */
-        /*
+        
         pc.printf("GainP = %lf, GainI = %lf, GainP = %lf, nog 10 seconden \n\r", GainP1, GainI1, GainD1);
         wait(7.0);
         pc.printf("nog 3 seconden \n\r");
         wait(3.0);
-        */
+        
         
         //BiQuad chains
-        bqc1.add( &HP_emg1 ).add( &Notch_emg1 );
-        bqc2.add( &HP_emg2 ).add( &Notch_emg2 );
-        bqc3.add( &HP_emg3 ).add( &Notch_emg3 );
-        bqc4.add( &HP_emg4 ).add( &Notch_emg4 );
+        bqc1.add( &HP_emg1 ).add( &Notch_emg1 );//.add( &LP1);
+        bqc2.add( &HP_emg2 ).add( &Notch_emg2 );//.add( &LP2);
+        bqc3.add( &HP_emg3 ).add( &Notch_emg3 );//.add( &LP3);
+        bqc4.add( &HP_emg4 ).add( &Notch_emg4 );//.add( &LP4);
         
         //Initialize array errors to 0
         for (int i = 0; i <= 9; i++){