Diff: main.cpp

Revision:

23:d1a3d537460f

Parent:

22:239075a92d33

Child:

24:f8482c47a385

Child:

26:9e21ce046d4e

--- a/main.cpp	Wed Oct 30 23:02:29 2019 +0000
+++ b/main.cpp	Thu Oct 31 12:11:54 2019 +0000
@@ -49,14 +49,17 @@
     double pi=3.14159265358979323846;
     double length_link1=18;
     double length_link2=15;
-    double x = 8.5;   // (of -2?) 8.5 < x < 30.5
+    double x = 0;   // (of -2?) 8.5 < x < 30.5
     double x_max= 30.5;
     double x_min= 8.5;
-    double x_vergroting=0.01;
-    double y = 7.5; // 7.5 < y < 27
+    double x_home=8.5;
+    double x_vergroting=0.3;
+    
+    double y = 0; // 7.5 < y < 27
     double y_max=27;
     double y_min=7.5;
-    double y_vergroting=0.01;
+    double y_home=7.5;
+    double y_vergroting=0.03;
     double a;
     double b;
     double c;
@@ -121,9 +124,16 @@
     AnalogIn    emg2(A2); //Rechterbeen
     AnalogIn    emg3(A3); //Linkerbeen
     
-    double emg0_max=0.0;
+    double emg0_max=0.0; float emg0_threshhold=0.0;
+    double emg1_max=0.0; double emg1_threshhold;
+    double emg2_max=0.0; double emg2_threshhold;
+    double emg3_max=0.0; double emg3_threshhold;
+    
+    double threshold_ratio=0.5;
     int emg_tijd=0; 
+    bool calibration_done=false;
     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
@@ -175,10 +185,9 @@
     // 4.1 Hidscope ****************************************************************
     void HIDScope() //voor HIDscope
     {
-        scope.set(0, x);
-        scope.set(1, y);
-   //     scope.set(2, emg_tijd); 
-   //     scope.set(3, emg3_signal);
+        scope.set(0, emg0_signal);
+        scope.set(1, emg0_max);
+        scope.set(2, x);
    //    scope.set(4, Ui_motor1);
    //    scope.set(5, Uk_motor1);
        
@@ -333,25 +342,71 @@
             motor2 = fabs(P_motor2);
         }            
     }
+void emg0_processing()
+{
+    emg0_raw_signal=emg0.read();
+
+    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;  
     
-    void motor1_controller(void)
-    {
-        error1_motor1 = (theta_motor1 - positie_motor1);
-        if (motor1_calibrated==true&&motor2_calibrated==true)
-            {
-                motor1_pwm();
-            } 
-        
-    }
+    emg0_threshhold = 0.5*emg0_max; //
+    
+    if (calibration_done && (emg0_signal>emg0_threshhold))
+    { emg0_active=true;}
+    else {emg0_active=false;}
+}     
+
+void emg1_processing()
+{
+    emg1_raw_signal=emg1.read();
+
+    high_emg1_signal  =     highfilter1.step(emg1_raw_signal);
+    rec_emg1_signal   =     abs(high_emg1_signal);    
+    low_rec_high_emg1_signal  =     LowPassFilter1.step(rec_emg1_signal);
+    emg1_signal = low_rec_high_emg1_signal;
+    
+    emg1_threshhold = 0.5*emg1_max; //
+    
+    if (calibration_done && (emg1_signal>emg1_threshhold))
+    { emg1_active=true;}
+    else {emg1_active=false;}
+    
+}    
+
+void emg2_processing()
+{
+    emg2_raw_signal=emg2.read();
 
-    void motor2_controller(void)
-    {
-        error1_motor2 = (theta_motor2 - positie_motor2);
-        if (motor1_calibrated==true&&motor2_calibrated==true)
-            {
-                motor2_pwm();
-            } 
-    }   
+    high_emg2_signal  =     highfilter2.step(emg2_raw_signal);
+    rec_emg2_signal   =     abs(high_emg2_signal);    
+    low_rec_high_emg2_signal  =     LowPassFilter2.step(rec_emg2_signal);
+    emg2_signal = low_rec_high_emg2_signal;
+    
+        emg2_threshhold = 0.5*emg2_max; //
+    
+    if (calibration_done && (emg2_signal>emg2_threshhold))
+    { emg2_active=true;}
+    else {emg2_active=false;}
+}    
+
+void emg3_processing()
+{
+    emg3_raw_signal=emg3.read();
+
+    high_emg3_signal  =     highfilter3.step(emg3_raw_signal);
+    rec_emg3_signal   =     abs(high_emg3_signal);    
+    low_rec_high_emg3_signal  =     LowPassFilter3.step(rec_emg3_signal);
+    emg3_signal = low_rec_high_emg3_signal;
+    
+    emg3_threshhold = 0.5*emg3_max; //
+    
+    if (calibration_done && (emg3_signal>emg3_threshhold))
+    { emg3_active=true;}
+    else {emg3_active=false;}
+}        
+ 
     
     double Angle_motor1()
     {
@@ -373,7 +428,7 @@
     {
         if (emg0_active==true)
         { if (x>x_max) {x=x_max;}
-          else { x=x+ x_vergroting; } 
+          else { x=x+ x_vergroting ; led_blue.write(0); }  
         }
         
         if (emg1_active==true)
@@ -394,50 +449,25 @@
         Angle_motor1();
         Angle_motor2();
     } 
-     
-void emg0_processing()
-{
-    emg0_raw_signal=emg0.read();
-
-    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;  
-    
-}     
-
-void emg1_processing()
-{
-    emg1_raw_signal=emg1.read();
 
-    high_emg1_signal  =     highfilter1.step(emg1_raw_signal);
-    rec_emg1_signal   =     abs(high_emg1_signal);    
-    low_rec_high_emg1_signal  =     LowPassFilter1.step(rec_emg1_signal);
-    emg1_signal = low_rec_high_emg1_signal;
-    
-}    
-
-void emg2_processing()
-{
-    emg2_raw_signal=emg2.read();
+    void motor1_controller(void)
+    {
+        error1_motor1 = (theta_motor1 - positie_motor1);
+        if (motor1_calibrated==true&&motor2_calibrated==true)
+            {
+                motor1_pwm();
+            } 
+        
+    }
 
-    high_emg2_signal  =     highfilter2.step(emg2_raw_signal);
-    rec_emg2_signal   =     abs(high_emg2_signal);    
-    low_rec_high_emg2_signal  =     LowPassFilter2.step(rec_emg2_signal);
-    emg2_signal = low_rec_high_emg2_signal;
-    
-}    
-
-void emg3_processing()
-{
-    emg3_raw_signal=emg3.read();
-
-    high_emg3_signal  =     highfilter3.step(emg3_raw_signal);
-    rec_emg3_signal   =     abs(high_emg3_signal);    
-    low_rec_high_emg3_signal  =     LowPassFilter3.step(rec_emg3_signal);
-    emg3_signal = low_rec_high_emg3_signal;
-    
-}    
+    void motor2_controller(void)
+    {
+        error1_motor2 = (theta_motor2 - positie_motor2);
+        if (motor1_calibrated==true&&motor2_calibrated==true)
+            {
+                motor2_pwm();
+            } 
+    }      
     
     // 4.3 State-Machine *******************************************************
 
@@ -472,6 +502,7 @@
             led_green.write(1);
             
             emg_tijd =0;
+            calibration_done=true;
             State=EMGCalibration;
             break;  
                        
@@ -490,10 +521,26 @@
                 if (emg0_signal > emg0_max)
                 {
                 emg0_max=emg0_signal;
-                }             
+                }     
+                
+                if (emg1_signal > emg1_max)
+                {
+                emg1_max=emg1_signal;
+                }   
+                
+                if (emg2_signal > emg2_max)
+                {
+                emg2_max=emg2_signal;
+                }  
+                
+                if (emg3_signal > emg3_max)
+                {
+                emg3_max=emg3_signal;
+                }         
             }
             else if (8000<emg_tijd)
             {led_green.write(0);
+               calibration_done=true; // later verplaatsen
                 if(button1==0) {State=StartWait;}
                 if(button2==0) {State=Homing;}
             }
@@ -507,7 +554,8 @@
             led2=1;
 
             homing_tijd++;
-            
+            x= x_home;
+            y= y_home;
             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;}
@@ -521,6 +569,7 @@
             led_blue.write(1);
             led_red.write(1);
             led_green.write(0);
+            led2=0;
             break; 
         
         case Demo:
@@ -564,6 +613,10 @@
 //******************************************************************************
 
 void main_loop() { //Beginning of main_loop()
+
+
+
+
 //    pc.printf("main_loop is running succesfully \r\n"); //confirmation that main_loop is running (als je dit erin zet krijg je elke duizendste dit bericht. Dit is niet gewenst)
 
 
@@ -587,8 +640,6 @@
 state_machine() ;
 // 5.3 Run controller(s) *******************************************************
 motor1_calibrated=true;motor2_calibrated=true;
-if (button1==0){emg0_active=true;}else {emg0_active=false;} //emg1_active=true;} else {emg0_active=false;emg1_active=false;}
-if (button2==0){emg2_active=true;}else {emg2_active=false;} //emg3_active=true;} else {emg2_active=false;emg3_active=false;}
 RKI();
 
 PID_controller_motor1(error_integral_motor1, error1_prev_motor1);
@@ -617,7 +668,7 @@
     led_green.write(1);
     led_red.write(1);
     led_blue.write(1);
-    State = Operating ; // 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       
     
@@ -625,6 +676,6 @@
     
     
 // 6.2 While loop in main function**********************************************   
-    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!!
+    while (true) { pc.printf("\r\n y = %f, theta_motor2 = %f, error1_motor2 = %f", y, theta_motor2, error1_motor2);} //Is not used but has to remain in the code!!
     
 }                  //Ending of Main() Function