Diff: main.cpp

Revision:

8:fa268d163bbd

Parent:

7:de221f894d3b

--- a/main.cpp	Fri Nov 02 08:15:25 2018 +0000
+++ b/main.cpp	Sat Nov 03 09:51:42 2018 +0000
@@ -1,18 +1,19 @@
 #include "mbed.h"
 #include "FastPWM.h"
-#include "MODSERIAL.h"
+//#include "MODSERIAL.h"
 #include "QEI.h"
-#include "HIDScope.h"
+//#include "HIDScope.h"
 #include "BiQuad.h"
 
 DigitalIn ButtonCal(D2);          //Button 1
+DigitalIn ButtonDemo(D1);;        //Button 2  
 DigitalOut gpo(D0);
 DigitalOut ledred(LED_RED);
 DigitalOut ledblue(LED_BLUE);
 DigitalOut ledgreen(LED_GREEN);
-AnalogIn pot1(A4);                  //POORTEN VERANDEREN
+//AnalogIn pot1(A4);                  //POORTEN VERANDEREN
 //AnalogIn pot2(A3);         //Beneden is I control op 0 gezet.   //POORTEN veranderen
-AnalogIn pot3(A5);                  //POORTEN VERANDEREN
+//AnalogIn pot3(A5);                  //POORTEN VERANDEREN
 QEI encoder1(D10, D11, NC, 32);
 QEI encoder2(D12, D13, NC, 32);
 FastPWM Motor1PWM(D6);
@@ -21,23 +22,24 @@
 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);
+//MODSERIAL pc(USBTX, USBRX);
 
 //HIDSCOPE          //Oppassen waar we HIDSCOPE aanroepen want nu voor meerdere dingen
-HIDScope    scope(6);
-Ticker      scopeTimer;
+//HIDScope    scope(6);
+//Ticker      scopeTimer;
 
 Ticker      EMGTicker;
 
@@ -126,7 +128,7 @@
 
 //States
 enum states {WaitModusState, EMGCalibrationState, NormalOperatingModusState, DemoModusState};
-states State = DemoModusState;
+states State = WaitModusState;
 
 
 //Calibration Time
@@ -138,12 +140,11 @@
 
 void ReadEMG()
 {
-/*
     EMGdata1 = a0.read(); // store values in the scope
     EMGdata2 = a1.read();
     EMGdata3 = a2.read();
     EMGdata4 = a3.read();
-*/
+
 }
 
 // EMG High pass filters
@@ -270,19 +271,19 @@
     }
         
 
-    scope.set(0, EMG_filt1);
+/*    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("xRef = %1f, yRef = %1f, q2Ref = %1f, q1Ref = %1f \n\r", xRef, yRef, q2Ref, q1Ref);
+        //pc.printf("xRef = %1f, yRef = %1f, q2Ref = %1f, q1Ref = %1f \n\r", xRef, yRef, q2Ref, q1Ref);
         count = 0;
     }
 }
@@ -573,7 +574,7 @@
     EMGCalibration();
     
     countcalibration++;
-    pc.printf("countcal = %i", countcalibration);
+    //pc.printf("countcal = %i", countcalibration);
     if (countcalibration >= (int)(CalibrationTime*1.0/TickerPeriod))
     {
         State = NormalOperatingModusState;
@@ -585,15 +586,16 @@
 void DemoModus()   // The ticker should call this function (in the switch statement)
 {    
 
-   GainP1 = 40.0;//Ppot.read()*100.0;
+   /*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();
+    DemonstrationPath();
+    //TestPath();
     inverse();
     ReadCurrentPosition();
     UpdateError();
@@ -608,7 +610,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", GainP2, GainI2, GainD2, 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;
     }
 }
@@ -659,14 +661,24 @@
     if (ButtonCal.read() == 0)
     {
         CalibrationButton();
-        pc.printf("print iets");
+   //     pc.printf("print iets");
+        Motor1PWM.write(0.0);
+        Motor2PWM.write(0.0);
+    }
+    if (ButtonDemo.read() == 0)
+    {
+        State = DemoModusState;
+        
+        ledred = 1;
+        ledgreen = 0;
+        ledblue = 0;
     }
         
     switch(State)
     {
         case WaitModusState: //aanmaken
                 EMG();
-                pc.printf("Wait \n\r");
+              //  pc.printf("Wait \n\r");
                 break;
         case EMGCalibrationState:
              CalibrationModus(); 
@@ -682,13 +694,14 @@
                 break;
         default :        
     }
+    
 }    
                                         
          
  
 int main()
 {
-        pc.baud(115200);
+        //pc.baud(115200);
         
         /*
         GainP1 = pot3.read() * 10;
@@ -696,9 +709,9 @@
         GainD1 = pot1.read();
         */
         
-        pc.printf("GainP = %lf, GainI = %lf, GainP = %lf, nog 10 seconden \n\r", GainP1, GainI1, GainD1);
+        //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");
+        //pc.printf("nog 3 seconden \n\r");
         wait(3.0);
         
         
@@ -715,8 +728,8 @@
              }
              
         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     
+        Motor1PWM.period(1.0/frequency_pwm); // T = 1/f
+        Motor2PWM.period(1.0/frequency_pwm); // T = 1/f     
         
        //Emg Calibratie button
         //ButtonCal.fall(&CalibrationButton);
@@ -728,7 +741,7 @@
         
         //Onderstaande stond in EMG deel, kijken hoe en wat met tickers!!
         // Attach the HIDScope::send method from the scope object to the timer at 50Hz
-        scopeTimer.attach_us(&scope, &HIDScope::send, 5e3);
+        //scopeTimer.attach_us(&scope, &HIDScope::send, 5e3);
         //EMGTicker.attach_us(EMG_filtering, 5e3);
         //.