Diff: THE.cpp

Revision:

17:65943f6e11dc

Parent:

16:37b491eac34b

Child:

18:db53ac017f50

--- a/THE.cpp	Thu Nov 01 11:14:44 2018 +0000
+++ b/THE.cpp	Thu Nov 01 12:00:18 2018 +0000
@@ -78,8 +78,7 @@
 int emg_calib=0; //After calibration this value will be 1, enabling the
 
 // EMG
-Ticker Filter_tick;
-Ticker MovAg_tick;
+Ticker process_tick;
 AnalogIn emg0_in( A0 );
 AnalogIn emg1_in( A1 );
 AnalogIn emg2_in( A2 );
@@ -90,54 +89,6 @@
 DigitalOut dirpin_2(D7); // direction of motor 2 (rotation)
 PwmOut pwmpin_2(D6); // PWM pin of motor 2
 
-// RKI related
-const double Ts = 0.001;// sample frequency 
-
-// Constants motor
-const double delta_t = 0.01;
-const double L1 = 370.0 / 2.0;
-const double L2 = 65.0 / 2.0;
-const double pi = 3.14159265359;
-const double alpha = (2.0 * pi) /(25.0*8400.0);
-const double beta = (((2.0 * L1) - (2.0 * L2)) * 20.0 * pi) / (305.0 * 8400.0);
-const double q1start = rotation_end_position * alpha;
-const double q2start = tower_1_position * beta; 
-const double q2end = tower_end_position * beta; 
-
-// Variables motors
-double out1;
-double out2;
-int counts1;
-int counts2;
-double vdesx;
-double vdesy;
-double q1;
-double q2;
-double MPe;
-double xe;
-double ye;
-double gamma;
-double dq1;
-double dq2;
-double dC1;
-double dC2;
-double pwm1;
-double pwm2;
-
-// PID rotation constants 
-double Rot_Kp = 1.5;
-double Rot_Ki = 0.1;
-double Rot_Kd = 0.48;
-double Rot_error = 0;
-double Rot_prev_error = 0;
-
-// PID translation constants
-const double Trans_Kp = 0.5;
-const double Trans_Ki = 0.5;
-const double Trans_Kd = 0.1;
-double Trans_error = 0;
-double Trans_prev_error = 0;
-
 // Extra stuff
 DigitalIn button_motorcal(SW2); // button for motor calibration, on mbed
 DigitalIn button_emergency(D7); // button for emergency mode, on bioshield
@@ -180,8 +131,56 @@
 volatile double tower_end_position = 0.1; // the tower which he reaches second
 volatile double rotation_start_position = 0.1; // the position where the rotation will remain
 volatile double position;
-float speed = 0.70;
-int dir = 0;
+volatile float speed = 0.70;
+volatile int dir = 0;
+
+// RKI related
+const double Ts = 0.001;// sample frequency 
+
+// Constants motor
+const double delta_t = 0.01;
+const double el_1 = 370.0 / 2.0;
+const double el_2 = 65.0 / 2.0;
+const double pi = 3.14159265359;
+const double alpha = (2.0 * pi) /(25.0*8400.0);
+const double beta = (((2.0 * el_1) - (2.0 * el_2)) * 20.0 * pi) / (305.0 * 8400.0);
+const double q1start = rotation_start_position * alpha;
+const double q2start = tower_1_position * beta; 
+const double q2end = tower_end_position * beta;
+
+// Variables motors
+volatile double desired_x;
+volatile double desired_y;
+volatile double out1;
+volatile double out2;
+volatile double vdesx;
+volatile double vdesy;
+volatile double q1;
+volatile double q2;
+volatile double MPe;
+volatile double xe;
+volatile double ye;
+volatile double gamma;
+volatile double dq1;
+volatile double dq2;
+volatile double dC1;
+volatile double dC2;
+volatile double pwm1;
+volatile double pwm2;
+
+// PID rotation constants 
+volatile double Rot_Kp = 1.5;
+volatile double Rot_Ki = 0.1;
+volatile double Rot_Kd = 0.48;
+volatile double Rot_error = 0.0;
+volatile double Rot_prev_error = 0.0;
+
+// PID translation constants
+const double Trans_Kp = 0.5;
+const double Trans_Ki = 0.5;
+const double Trans_Kd = 0.1;
+volatile double Trans_error = 0.0;
+volatile double Trans_prev_error = 0.0;
 
 // states
 enum states {WAIT, MOTOR_CAL, EMG_CAL, START, OPERATING, FAILURE, DEMO}; // states the robot can be in
@@ -309,6 +308,13 @@
     scope.send(); 
     }
 
+// This must be applied to all emg signals coming in
+void processing_emg()
+{
+    filtering();
+    MovAg();
+    }
+    
 // WAIT
 // To do nothing
 void wait_mode()
@@ -535,10 +541,7 @@
     {
         led_blue == 0;
         led_red == 1;
-        led_green == 1; 
-        
-        MovAg_tick.attach(&MovAg,T); // Moving average calculation every T sec.
-        
+        led_green == 1;      
         
         pc.printf("g is %i\n\r",g);
         pc.printf("Average0 = %f , Average1 = %f, Average2 = %f \n\r",Average0, Average1, Average2);
@@ -573,27 +576,6 @@
         led_blue == 1;
         led_green == 1;
     }
-    
-// PID controller
-// To control the input signal before it goes into the motor control
-// Simon mee bezig
-// PID execution
-double PID_control(double error, const double kp, const double ki, const double kd, double &error_int, double &error_prev)
-{ 
-    // P control
-    double u_k = kp * error;
-
-    // I control
-    error_int = error_int + (Ts * error);
-    double u_i = ki * error_int;
-
-    // D control
-    double error_deriv = (error - error_prev);
-    double u_d = kd * error_deriv;
-    error_prev = error;
-
-    return u_k + u_i + u_d;
-    }
 
 // START
 // To move the robot to the starting position: middle
@@ -620,7 +602,110 @@
 
 // OPERATING
 // To control the robot with EMG signals
-// Kenneth bezig met motor aansturen
+// Function for using muscle for direction control    
+void Directioncontrol()
+{
+    enum direction {Pos_RB, Pos_LB, Pos_RO, Pos_LO};
+    direction currentdirection = Pos_RB;
+    bool directionchanged = true;
+    
+    switch (currentdirection)
+        {
+            case Pos_RB:
+            
+            if (directionchanged)
+            {
+                desired_x = desired_x;
+                desired_y = desired_y;
+                directionchanged = false;
+                }
+                
+            if (Average2 > Threshold2)
+            {
+                currentdirection = Pos_LB;
+                pc.printf("\r\n direction = Pos_LB\r\n");
+                directionchanged = true;
+                }
+                
+            break;    
+            
+            case Pos_LB:
+            
+            if(directionchanged)
+            {
+                desired_x = desired_x * -1.0;
+                desired_y = desired_y;
+                directionchanged = false;
+                }
+            
+            if (Average2 > Threshold2)
+            {
+                currentdirection = Pos_RO;
+                pc.printf("\r\n direction = Pos_RO\r\n");
+                directionchanged = true;
+                }
+                
+            break;
+                        
+            case Pos_RO:
+            
+            if(directionchanged)
+            {
+                desired_x = desired_x;
+                Average1 = Average1 * -1.0;
+                directionchanged = false;
+                }
+                    
+            if (Average2 > Threshold2)
+            {
+                currentdirection = Pos_LO;
+                pc.printf("\r\n direction = Pos_LO\r\n");
+                directionchanged = true;
+                }
+                
+            break;
+                
+            case Pos_LO:
+            
+            if(directionchanged)
+            {
+                desired_x = desired_x * -1.0;
+                desired_y = desired_y * -1.0;
+                directionchanged = false;
+                }
+            
+            if (Average2 > Threshold2)
+            {
+                currentdirection = Pos_RB;
+                pc.printf("\r\n direction = Pos_RB\r\n");
+                directionchanged = true;
+                }
+                
+            break;
+            }
+    }
+    
+    
+// PID controller
+// To control the input signal before it goes into the motor control
+// PID execution
+double PID_control(volatile double error, const double kp, const double ki, const double kd, volatile double &error_int, volatile double &error_prev)
+{ 
+    // P control
+    double u_k = kp * error;
+
+    // I control
+    error_int = error_int + (Ts * error);
+    double u_i = ki * error_int;
+
+    // D control
+    double error_deriv = (error - error_prev);
+    double u_d = kd * error_deriv;
+    error_prev = error;
+
+    return u_k + u_i + u_d;
+    }
+
 void boundaries()
 {
     double q2tot = q2 + dq2;
@@ -638,16 +723,32 @@
 void motor_control()
 {
     // servocontrol(); // make sure the servo is used in this mode, maybe attach to a ticker?
-    out1 = (pot1*2.0f)-1.0f; //control x-direction
-    out2 = (pot2*2.0f)-1.0f; //control y-direction
-    counts1 = encoder1.getPulses(); //counts encoder 1
-    counts2 = encoder2.getPulses(); //counts encoder 2
+    
+    // filtering emg
+    
+    if (Average0 > Threshold0)
+    {
+        desired_x = 1.0;
+        }
+    
+    if (Average1 > Threshold1)
+    {
+        desired_y = 1.0;
+        }
+        
+        
+    // calling functions
+    Directioncontrol();    
+    
+    // motor control
+    out1 = (desired_x* 2.0f) - 1.0f; //control x-direction
+    out2 = (desired_y * 2.0f) - 1.0f; //control y-direction
     vdesx = out1 * 20.0; //speed x-direction
     vdesy = out2 * 20.0; //speed y-direction
 
-    q1 = counts1 * alpha + q1start; //counts to rotation (rad) 
-    q2 = counts2 * beta + q2start; //counts to translation (mm)
-    MPe = L1 - L2 + q2; //x location end effector, x-axis along the translation
+    q1 = Counts1(counts1) * alpha + q1start; //counts to rotation (rad) 
+    q2 = Counts2(counts2)* beta + q2start; //counts to translation (mm)
+    MPe = el_1 - el_2 + q2; //x location end effector, x-axis along the translation
     xe = cos(q1) * MPe; //x location in frame 0
     ye = sin(q1) * MPe; //y location in frame 0
     gamma = 1.0 /((-1.0 * ye * sin(q1)) - (xe * cos(q1))); //(1 / det(J'')inverse)
@@ -658,10 +759,10 @@
     dC2 = PID_control( dq2, Trans_Kp, Trans_Ki, Trans_Kd, Trans_error, Trans_prev_error) / beta; //target translation to counts
     pwm1 = 3.0 * (dC1 / delta_t) / 8400.0; //
     pwm2 = 3.0 * (dC2 / delta_t) / 8400.0; //
-    dirpin.write(pwm1 < 0);                    
-    pwmpin = fabs (pwm1);                    
-    dirpin2.write(pwm2 < 0);
-    pwmpin2 = fabs (pwm2);
+    dirpin_1.write(pwm1 < 0);                    
+    pwmpin_1 = fabs (pwm1);                    
+    dirpin_2.write(pwm2 < 0);
+    pwmpin_2 = fabs (pwm2);
     }   
 
 // DEMO
@@ -695,7 +796,7 @@
     pc.baud(115200); // For TeraTerm, the baudrate, set also in TeraTerm itself! 
     pc.printf("Start code\r\n"); // To check if the program starts 
     pwmpin_1.period_us(60); // Setting period for PWM
-    Filter_tick.attach(&filtering,T); // EMG signals must be filtered all the time! EMG signals filtered every T sec.
+    process_tick.attach(&processing_emg,T); // EMG signals must be filtered all the time! EMG signals filtered every T sec.
     
     while(true){
         // timer
@@ -842,37 +943,41 @@
             if(StateChanged) // so if StateChanged is true
             {
                 // Execute OPERATING mode
-                while(1)
+                while(true)
                 {
                     motor_control();
                     pc.printf("PWM_rot = %f PWM_trans = %f VdesX = %f VdesY = %f \n\r",pwm1,pwm2,vdesx,vdesy);
+                    
+                    if(button_emergency == false) // condition for OPERATING --> FAILURE; button_emergency press
+                    {
+                        CurrentState = FAILURE;
+                        pc.printf("State is FAILURE\r\n");
+                        StateChanged = true;
+                        break;
+                        }
+                
+                    if(button_demo == false) // condition for OPERATING --> DEMO; button_demo press
+                    {
+                        CurrentState = DEMO;
+                        pc.printf("State is DEMO\r\n");
+                        StateChanged = true;
+                        break;
+                        }
+                
+                    if(button_wait == false) // condition OPERATING --> WAIT; button_wait press  
+                    {
+                        CurrentState = WAIT;
+                        pc.printf("State is WAIT\r\n");
+                        StateChanged = true;
+                        break;
+                        }
+                    
                     wait(delta_t);
                     }
                 
                 StateChanged = false; // state is still OPERATING
                 }
             
-            if(button_emergency == false) // condition for OPERATING --> FAILURE; button_emergency press
-            {
-                CurrentState = FAILURE;
-                pc.printf("State is FAILURE\r\n");
-                StateChanged = true;
-                }
-                
-            if(button_demo == false) // condition for OPERATING --> DEMO; button_demo press
-            {
-                CurrentState = DEMO;
-                pc.printf("State is DEMO\r\n");
-                StateChanged = true;
-                }
-                
-            if(button_wait == false) // condition OPERATING --> WAIT; button_wait press  
-            {
-                CurrentState = WAIT;
-                pc.printf("State is WAIT\r\n");
-                StateChanged = true;
-                }
-                
             break;
             
             case FAILURE: