Diff: main.cpp

Revision:

15:40dd74bd48d1

Parent:

14:059fd8f6cbfd

Child:

16:ac4e3730f61f

--- a/main.cpp	Thu Nov 01 20:44:47 2018 +0000
+++ b/main.cpp	Thu Nov 01 21:57:28 2018 +0000
@@ -32,16 +32,16 @@
 // EMG input en start value of filtered EMG
 AnalogIn    emg1_raw( A0 );
 AnalogIn    emg2_raw( A1 );
-AnalogIn  potmeter1(PTC11);
+AnalogIn potmeter1(PTC11);
 AnalogIn potmeter2(PTC10);
 
 // Declare timers and Tickers
 Timer       timer;                        // Timer for counting time in this state
-Ticker      WriteValues;            // Ticker to show values of velocity to screen
+//Ticker      WriteValues;            // Ticker to show values of velocity to screen
 Ticker      StateMachine;
 //Ticker      sample_EMGtoHIDscope;   // Ticker to send the EMG signals to screen
 //HIDScope    scope(4);               //Number of channels which needs to be send to the HIDScope
-Ticker      sample;          // Ticker for reading out EMG
+//Ticker      sample;          // Ticker for reading out EMG
 
 // Set up ProcessStateMachine
 enum states {WAITING, MOTOR_ANGLE_CLBRT, EMG_CLBRT, HOMING, WAITING4SIGNAL, MOVE_W_EMG, MOVE_W_KNOPJES, MOVE_W_DEMO, FAILURE_MODE};
@@ -52,7 +52,7 @@
 
 // Global variables
 char c;
-const float fs = 1/1024;
+//const float fs = 1/1024;
 int counts1; 
 int counts2;
 float theta1;
@@ -68,10 +68,10 @@
 float time_in_state;
 int need_to_move_1;                     // Does the robot needs to move in the first direction?
 int need_to_move_2;                     // Does the robot needs to move in the second direction?
-volatile double EMG_calibrated_max_1 = 0.00000;  // Maximum value of the first EMG signal found in the calibration state.
-volatile double EMG_calibrated_max_2 = 0.00000;  // Maximum value of the second EMG signal found in the calibration state.
-volatile double emg1_cal;              //measured value of the first emg
-volatile double emg2_cal;              //measured value of the second emg
+volatile double EMG_calibrated_max_1 = 2.00000;  // Maximum value of the first EMG signal found in the calibration state.
+volatile double EMG_calibrated_max_2 = 2.00000;  // Maximum value of the second EMG signal found in the calibration state.
+volatile double emg1_filtered;              //measured value of the first emg
+volatile double emg2_filtered;              //measured value of the second emg
 const double x0 = 80.0; //zero x position after homing
 const double y0 = 141.0; //zero y position after homing
 volatile double  setpointx = x0;
@@ -166,14 +166,24 @@
         directionM2 = U2 > 0.0f;
 } 
 
+//function to change the moving direction of the setpoint
+void ChangeDirectionX(){
+    dirx = -1*dirx;
+    }
+    
+void ChangeDirectionY(){
+    diry = -1*diry;
+    } 
+
 // Read EMG
-void ReadEMG()
+void sample()
 {
-    emg1_cal = FilterDesign(emg1_raw.read());
-    emg2_cal = FilterDesign2(emg2_raw.read());
-    pc.printf("emg1_cal = %g, emg2_cal = %g \n\r", emg1_cal, emg2_cal);
+    emg1_filtered = FilterDesign(emg1_raw.read());
+    emg2_filtered = FilterDesign2(emg2_raw.read());
+    //pc.printf("emg1_cal = %g, emg2_cal = %g \n\r", emg1_cal, emg2_cal);
 }
 
+/*
 // EMG calibration
 void EMG_calibration()
 {
@@ -191,6 +201,7 @@
         wait(0.5f);
         }
 }
+*/
 
 // Inverse Kinematics
 double makeAngleq1(double x, double y){
@@ -265,37 +276,39 @@
 
 // Determination of setpoint
 void determineEMGset(){
-    const double v = 0.1; //moving speed of setpoint 
-    setpointx = setpointx + dirx*sx*v;
-    setpointy = setpointy + diry*sy*v;
-    }
-void ChangeDirectionX(){
-    dirx = -1*dirx;
-    }
-void ChangeDirectionY(){
-    diry = -1*diry;
-    }  
+    setpointx = setpointx + dirx*need_to_move_1*v;
+    setpointy = setpointy + diry*need_to_move_2*v;
+    } 
 
 // Motoraansturing voor EMG signalen   
-/** 
-void motoraansturing()
+
+void motoraansturingEMG()
 {
+    sample();
     determineEMGset();
     q1_diff = makeAngleq1(setpointx, setpointy);
     q2_diff = makeAngleq2(setpointx, setpointy);
-    ReadEncoder1();
-    ReadEncoder2();           
-    double error2 = q2_diff - theta2; 
-    double error1 = q1_diff - theta1;          // Setpoint error, te behalen setpoint minus de huidige positie van de as.
-    U1 = PI_controller1(error1);               // Voltage dat naar de motor gestuurd wordt. 
-    U2 = PI_controller2(error2);   
-    pc.printf("U1 = %g, U2 = %g \n\r", U1, U2);
-    motor1_pwm.write(fabs(U1));                // Motor aansturen
-    directionM1 = U1 > 0.0f;                   // Richting van de motor bepalen
+    //q1_diff_final = makeAngleq1(xfinal, yfinal);
+    //q2_diff_final = makeAngleq2(xfinal, yfinal);
+
+    theta2 = counts2angle2();           
+    error2 = q2_diff - theta2;
+    theta1 = counts2angle1();  
+    error1 = q1_diff - theta1;          // Setpoint error, te behalen setpoint minus de huidige positie van de as.
+    
+    //errors die de setpoints bepalen
+    //error1_final = q1_diff_final - theta1;
+    //error2_final = q2_diff_final - theta2;
+
+    U1 = PID_controller1(error1);       // Voltage dat naar de motor gestuurd wordt. 
+    U2 = PID_controller2(error2);   
+    
+    motor1_pwm.write(fabs(U1));         // Motor aansturen
+    directionM1 = U1 > 0.0f;            // Richting van de motor bepalen
     motor2_pwm.write(fabs(U2));
     directionM2 = U2 > 0.0f; 
 }
-**/ 
+
 double determinedemosetx(double setpointx, double setpointy)
 {
 
@@ -732,42 +745,34 @@
         break; 
         
  
-/**           
+          
         case MOVE_W_EMG:
-        // Description:
-        // In this state the robot will be controlled by use of 
-        // EMG-signals. 
+        
+        motor1_pwm.period_us(60);                // Period is 60 microseconde
+        motor2_pwm.period_us(60);
+        led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led BLUE
         
-        // Actions                    
-        led_red = 1; led_green = 0; led_blue = 1;   // Colouring the led GREEN
-        ReadEMG();
-        if (stateChanged){
-        //ReadEMG();
-        //pc.printf(" emg1 = %g, emg2 = %g \n\r ", emg1_cal, emg2_cal);     
-        if (emg1_cal >= (threshold_EMG*EMG_calibrated_max_1)){
-            sx = 1; // The robot does have to move
-            }
-        else {
-            sx = 0; // If the robot does not have to move
-            }
+        // Actions
+        if (stateChanged) {
+            motoraansturingEMG();
+            stateChanged = true;
+        }
+      /*
+        if (buttonx == 0)  {
+            dirx = -1*dirx;
+        }
 
-        if(emg1_cal >= threshold_EMG*EMG_calibrated_max_2){
-            sy = 1;
-            }
-        else {
-            sy = 0;
-            }   
-            
-        motoraansturing();     
-        stateChanged = true;
+        if (buttony == 0)  {
+            diry = -1*diry;
         }
+        */ 
         
         // State transition logic
         if (button_clbrt_home == 0) 
         {
-            currentState = MOTOR_ANGLE_CLBRT;
+            currentState = HOMING;
             stateChanged = true;
-            pc.printf("Starting Calibration \n\r");
+            pc.printf("Starting Homing \n\r");
         }        
         else if (Fail_button == 0)
         {
@@ -775,7 +780,7 @@
             stateChanged = true;
         }
         break; 
-**/             
+            
                 
         case FAILURE_MODE:
         // Description:
@@ -842,14 +847,15 @@
     StateMachine.attach(&ProcessStateMachine, 0.005f);   // Run statemachine 200 times per second
     
     
-    InterruptIn directionx(SW3);
-    directionx.fall(ChangeDirectionX); //change the direction of the setpoint in x direction
-    InterruptIn directiony(SW2);
-    directiony.fall(ChangeDirectionY); //change the direction of the setpoint in y direction
+    InterruptIn buttonx(D2);
+    buttonx.fall(ChangeDirectionX); //change the direction of the setpoint in x direction
+    InterruptIn buttony(D3);
+    buttony.fall(ChangeDirectionY); //change the direction of the setpoint in y direction
         
     while (true) {
         
         if (currentState == MOVE_W_DEMO) {
+            pc.printf("Current state is move with demo");
             pc.printf("Setpointx: %0.2f, Setpointy: %0.2f, q1_diff: %0.2f, q2_diff: %0.2f, error1: %0.2f, error2: %0.2f, U1: %0.2f, U2: %0.2f\r\n", setpointx,setpointy,q1_diff,q2_diff,error1,error2,U1,U2);
             
             if (track == 1) {
@@ -866,6 +872,22 @@
                 pc.printf("Gaat naar positie 4\r\n");
             }
         }
+        
+        if (currentState == MOVE_W_EMG) {
+            if (emg1_filtered >= (threshold_EMG*EMG_calibrated_max_1)){
+                need_to_move_1 = 1; // The robot does have to move
+                }
+            else {
+                need_to_move_1 = 0; // If the robot does not have to move
+                }
+    
+            if(emg2_filtered >= threshold_EMG*EMG_calibrated_max_2){
+                need_to_move_2 = 1;
+                }
+            else {
+                need_to_move_2 = 0;
+                }
+        }
 
         wait(0.5f);
     }