Diff: main.cpp

Revision:

18:8bfc1821d412

Parent:

17:f87e5d6c87f4

Child:

19:483fc61778f0

--- a/main.cpp	Wed Oct 30 10:54:50 2019 +0000
+++ b/main.cpp	Wed Oct 30 13:51:33 2019 +0000
@@ -45,6 +45,9 @@
 
 
 // 3.3 EMG Variables **********************************************************
+static BiQuad LowPassFilter_motor1( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 ); 
+static BiQuad LowPassFilter_motor2( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 ); 
+
 static BiQuad highfilter0(9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01);   
 static BiQuad LowPassFilter0( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 ); 
     
@@ -62,7 +65,8 @@
 double rec_emg0_signal ; double rec_emg1_signal ;  double rec_emg2_signal ;  double rec_emg3_signal ;   
 double low_rec_high_emg0_signal ; double low_rec_high_emg1_signal ; double low_rec_high_emg2_signal ; double low_rec_high_emg3_signal ;
 double emg0_signal ; double emg1_signal ; double emg2_signal ; double emg3_signal ;
- 
+
+
 
 // 3.4 Hardware ***************************************************************
     //3.4a Leds
@@ -94,6 +98,9 @@
     AnalogIn    emg2(A2); //Rechterbeen
     AnalogIn    emg3(A3); //Linkerbeen
     
+    double emg0_max=0.0;
+    int emg_tijd=0; 
+    int homing_tijd=0;
 // 3.5 Motor 1 variables ***************************************************************   
     //3.5a PID-controller motor 1
     double counts_per_rad_motor1 = (131.25*32)/(2*3.14159265359); // (gear ratio * counts per revolution) / (2* pi) = ~668.45 counts per rad
@@ -146,9 +153,9 @@
     void HIDScope() //voor HIDscope
     {
         scope.set(0, emg0_signal);
-        scope.set(1, emg1_signal);
-        scope.set(2, emg2_signal); 
-        scope.set(3, emg3_signal);
+        scope.set(1, emg0_max);
+        scope.set(2, emg_tijd); 
+   //     scope.set(3, emg3_signal);
    //    scope.set(4, Ui_motor1);
    //    scope.set(5, Uk_motor1);
        
@@ -226,7 +233,7 @@
         //Derivative part 
         Kd_motor1 = 0.001 ;// moet nog getweaked worden  
         error1_derivative_motor1 = (error1_motor1-error1_prev_motor1)/Ts; // (Fout - de vorige fout) / tijdstap = afgeleide
-        error1_derivative_filtered_motor1 = LowPassFilter.step(error1_derivative_motor1); //derivative wordt gefiltered
+        error1_derivative_filtered_motor1 = LowPassFilter_motor1.step(error1_derivative_motor1); //derivative wordt gefiltered
         Ud_motor1 = Kd_motor1 * error1_derivative_filtered_motor1;           // (afgeleide gain) * (afgeleide gefilterde fout) 
         error1_prev_motor1 = error1_motor1;
 
@@ -252,7 +259,7 @@
         //Derivative part 
         Kd_motor2 = 0.001 ;// moet nog getweaked worden  
         error1_derivative_motor2 = (error1_motor2  -  error1_prev_motor2)/Ts;
-        error1_derivative_filtered_motor2 = LowPassFilter.step(error1_derivative_motor2); //derivative wordt gefiltered, dit later aanpassen
+        error1_derivative_filtered_motor2 = LowPassFilter_motor2.step(error1_derivative_motor2); //derivative wordt gefiltered, dit later aanpassen
         Ud_motor2 = Kd_motor2 * error1_derivative_filtered_motor2;
         error1_prev_motor2 = error1_motor2;
 
@@ -330,7 +337,7 @@
     high_emg0_signal  =     highfilter0.step(emg0_raw_signal);
     rec_emg0_signal   =     abs(high_emg0_signal);    
     low_rec_high_emg0_signal  =     LowPassFilter0.step(rec_emg0_signal);
-    emg0_signal = low_rec_high_emg0_signal;
+    emg0_signal = low_rec_high_emg0_signal;  
     
 }     
 
@@ -399,6 +406,7 @@
             led_red.write(1);
             led_green.write(1);
             
+            emg_tijd =0;
             State=EMGCalibration;
             break;  
                        
@@ -406,19 +414,46 @@
    //         pc.printf("\r\n State: EMGCalibration");
             led_blue.write(1);
             led_red.write(1);
-            led_green.write(0);
-
-            Yref_motor1=5000;
-            Yref_motor2=2000;
-            State=Homing; 
+            led_green.write(1);
+            
+            emg_tijd++;
+            
+            if (0<=emg_tijd&&emg_tijd<=5000)
+            { led_green.write(0); }
+            else if (5000<emg_tijd&&emg_tijd<8000)
+            {   led_green.write(1);
+                if (emg0_signal > emg0_max)
+                {
+                emg0_max=emg0_signal;
+                }             
+            }
+            else if (8000<emg_tijd)
+            {led_green.write(0);
+                if(button1==0) {State=StartWait;}
+                if(button2==0) {State=Homing;}
+            }
+            
+            
+            
+//            Yref_motor1=5000;
+//            Yref_motor2=2000;
+//            State=Homing; 
             break; 
         case Homing: 
        //     pc.printf("\r\n State: Homing");
+            led_blue.write(0);
+            led_red.write(1);
             led_green.write(1);
             led2=1;
-            if(button1==0) {State=Operating;}
-            if(button2==0) {State=Demo;} 
 
+            homing_tijd++;
+            
+            if (homing_tijd>1000) //Voorkomt dat het automatisch naar Demo springt, omdat je bij de vorige nog knop 2 hebt ingedrukt
+            {
+                if(button1==0) {State=Operating;}
+                if(button2==0) {State=Demo;} 
+            }
+            
             break; 
   
         case Operating:
@@ -513,14 +548,14 @@
     led_green.write(1);
     led_red.write(1);
     led_blue.write(1);
-    State = StartWait ; // veranderen naar MotorCalibration;
+    State = EMGCalibration ; // veranderen naar MotorCalibration;
     ticker_hidscope.attach(&HIDScope, 0.001); //Ticker for Hidscope, different frequency compared to motors
     ticker_mainloop.attach(&main_loop,0.001); // change back to 0.001f //Run the function main_loop 1000 times per second       
     
-    motor_calibration();
+   // motor_calibration();
     
     
 // 6.2 While loop in main function**********************************************   
-    while (true) { } //Is not used but has to remain in the code!!
+    while (true) { pc.printf("\r\n emg_tijd = %d, homing_tijd= %d", emg_tijd, homing_tijd); } //Is not used but has to remain in the code!!
     
 }                  //Ending of Main() Function