Diff: main.cpp

Revision:

5:8e326d07f125

Parent:

4:93c4e826d11d

Child:

6:405ec2bba6d0

--- a/main.cpp	Wed Oct 31 12:05:10 2018 +0000
+++ b/main.cpp	Wed Oct 31 16:08:45 2018 +0000
@@ -5,11 +5,13 @@
 #include "HIDScope.h"
 #include "BiQuad.h"
 
-InterruptIn buttoncal(D2);
+DigitalIn ButtonCal(D2);          //Button 1
 DigitalOut gpo(D0);
-DigitalOut led(LED_RED);
+DigitalOut ledred(LED_RED);
+DigitalOut ledblue(LED_BLUE);
+DigitalOut ledgreen(LED_GREEN);
 AnalogIn pot1(A4);                  //POORTEN VERANDEREN
-AnalogIn pot2(A3);         //Beneden is I control op 0 gezet.   //POORTEN veranderen
+//AnalogIn pot2(A3);         //Beneden is I control op 0 gezet.   //POORTEN veranderen
 AnalogIn pot3(A5);                  //POORTEN VERANDEREN
 QEI encoder1(D10, D11, NC, 32);
 QEI encoder2(D12, D13, NC, 32);
@@ -21,13 +23,13 @@
 AnalogIn    a0(A0);
 AnalogIn    a1(A1);
 AnalogIn    a2(A2);
-//AnalogIn    a3(A3);
+AnalogIn    a3(A3);
 
 MODSERIAL pc(USBTX, USBRX);
 
 //HIDSCOPE          //Oppassen waar we HIDSCOPE aanroepen want nu voor meerdere dingen
-//HIDScope    scope(4);
-//Ticker      scopeTimer;
+HIDScope    scope(4);
+Ticker      scopeTimer;
 
 Ticker      EMGTicker;
 
@@ -100,12 +102,20 @@
 int countstep = 0;
 
 //Demo variabelen
-double vxMax = 2.0;
-double vyMax = 2.0;
+double vxMax = 4.0;
+double vyMax = 4.0;
 int DemoStage = 0;
 
+//States
+enum states {WaitModusState, EMGCalibrationState, NormalOperatingModusState, DemoModusState};
+states State = WaitModusState;
 
 
+//Calibration Time
+
+int countcalibration = 0;
+double CalibrationTime = 20.0; //Tijd om te calibreren seconden
+
 //EMGDINGEN
 
 void ReadEMG()
@@ -113,7 +123,7 @@
     EMGdata1 = a0.read(); // store values in the scope
     EMGdata2 = a1.read();
     EMGdata3 = a2.read();
-    EMGdata4 = 0.0; //a3.read();
+    EMGdata4 = a3.read();
 }
 
 // EMG High pass filters
@@ -230,18 +240,27 @@
 
     unit_vX = filt2kin (EMG_filt1, EMG_filt2, EMG_max1, EMG_max2);
     unit_vY   = filt2kin (EMG_filt3, EMG_filt4, EMG_max3, EMG_max4);
+    if (fabs(unit_vX)<0.1)
+    {
+        unit_vX = 0.0;
+    }
+    if (fabs(unit_vY)<0.1)
+    {
+        unit_vY = 0.0;
+    }
+        
 
-    //scope.set(0, unit_Vx);
-    //scope.set(1, unit_Vy);
-    //scope.set(2, EMG_filt3);
-    //scope.set(3, EMG_filt4);
+    scope.set(0, unit_vX);
+    scope.set(1, unit_vY);
+    scope.set(2, EMG_filt1);
+    scope.set(3, EMG_filt2);
     
 
     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("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);
         count = 0;
     }
 }
@@ -395,13 +414,6 @@
 }
 
 
-void HomingSystem()     //Manually place the arm in position q1 = 90degrees, q2 = 0 degrees
-{
-    q1Pos = 3.1416 / 2.0;
-    q2Pos = 0.0;
-    q1Ref = 3.1416 / 2.0;
-    q2Ref = 0.0;
-}
 
 void DemonstrationPath()
 {
@@ -455,14 +467,73 @@
 
 
 
+void CalibrationButton()
+{
+    ledred = 1;
+    ledgreen = 1;
+    ledblue = 0;
+    
+    EMG_max1 = 0.0001;
+    EMG_max2 = 0.0001;
+    EMG_max3 = 0.0001;
+    EMG_max4 = 0.0001;
+   
+    State = EMGCalibrationState;
+    
+        
+}
+
+void EMGCalibration()
+{
+    if     (0.95*EMG_filt1>EMG_max1)
+    {
+        EMG_max1 = 0.95*EMG_filt1;
+    }
+    if     (0.95*EMG_filt2>EMG_max2)
+    
+    {
+        EMG_max2 = 0.95*EMG_filt2;
+    }
+    
+    if     (0.95*EMG_filt3>EMG_max3)
+    {
+            EMG_max3 = 0.95*EMG_filt3;
+    }
+    if     (0.95*EMG_filt4>EMG_max4)
+    {
+        EMG_max4 = 0.95*EMG_filt4;
+    }
+      
+}
+
+
+
 void CalibrationModus()
 {
-    EMG();
-    //EMGCalibration();
+    
+    EMG();    
+    EMGCalibration();
+    
+    countcalibration++;
+    pc.printf("countcal = %i", countcalibration);
+    if (countcalibration >= (int)(CalibrationTime*1.0/TickerPeriod))
+    {
+        State = NormalOperatingModusState;
+        countcalibration = 0;
+    }
+    
 }
 
 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() ;
+     
     DemonstrationPath();
     inverse();
     ReadCurrentPosition();
@@ -470,10 +541,15 @@
     PIDControl();
     MotorControl();
     
+    //scope.set(0, q1Pos);
+   // scope.set(1, q1Ref);
+    //scope.set(2, q2Pos);
+    //scope.set(3, q2Ref);
+    
     count++;
     if (count ==400)
     {
-    pc.printf("q1Pos = %lf, q2Pos = %lf, q1Ref = %lf, q2Ref = %lf,   xRef = %lf, yRef = %lf \n\r", 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", GainP1, GainI1, GainD1, q1Pos, q2Pos, q1Ref, q2Ref, xRef, yRef); 
     count = 0;
     }
 }
@@ -481,6 +557,10 @@
 
 void NormalOperatingModus()
 {
+     ledred = 1;
+     ledgreen = 0;
+     ledblue = 1;
+     
      EMG();
      InverseKinematics();
      ReadCurrentPosition();
@@ -499,7 +579,7 @@
      counter++;
      if (counter == 400) // print 1x per seconde waardes.
     {
-        pc.printf("GainP1 = %1f, GainI1 = %1f, GainD1 = %1f, q1Pos = %lf, q1Ref = %1f \n\r", GainP1, GainI1, GainD1, q1Pos, q1Ref);
+        pc.printf("xRef = %lf, q1Pos = %lf, q1Ref = %1f \n\r", xRef, q1Pos, q1Ref);
         counter = 0;   
     }
      if (countstep == 4000)
@@ -511,38 +591,57 @@
     
 }
 
-//enum states {WaitModus, EMGCalibration, NormalOperatingModus, DemonstrationModus};
+
  
-/*void StateMachine()
+void StateMachine()
 {
+    
+    
+    if (ButtonCal.read() == 0)
+    {
+        CalibrationButton();
+        pc.printf("print iets");
+    }
+        
     switch(State)
     {
-        case WaitModus: //aanmaken
+        case WaitModusState: //aanmaken
                 EMG();
+                pc.printf("Wait \n\r");
                 break;
-        case EMGCalibration:
-             EMGCalibration();           
+        case EMGCalibrationState:
+             CalibrationModus(); 
+             //pc.printf("EMG CAL \n\r");          
                 break;
-        case NormalOperatingModus:
+        case NormalOperatingModusState:
                 NormalOperatingModus();
+                //pc.printf("NOMS \n\r");
                 break;
-        case DemoModus:
-             DemoModus();      //Goede naam geven..
+        case DemoModusState:
+             DemoModus();    
+             pc.printf("Demo \n\r");  
                 break;
         default :        
     }
 }    
-*/                                         
+                                        
          
  
 int main()
 {
         pc.baud(115200);
         
-        pc.printf("nog 10 seconden \n\r");
+        /*
+        GainP1 = pot3.read() * 10;
+        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 );
@@ -560,14 +659,17 @@
         Motor1PWM.period(1.0/frequency_pwm); // T = 1/f
         Motor2PWM.period(1.0/frequency_pwm); // T = 1/f     
         
-        HomingSystem();
+       //Emg Calibratie button
+        //ButtonCal.fall(&CalibrationButton);
+        
         
-        Kikker.attach(DemoModus, TickerPeriod);
-        //scopeTimer.attach_us(&scope, &HIDScope::send, 2e4);
+        
+        Kikker.attach(StateMachine, TickerPeriod);
+       // scopeTimer.attach_us(&scope, &HIDScope::send, 2e4);
         
         //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);
         //.