Diff: main.cpp

Revision:

14:059fd8f6cbfd

Parent:

13:a2e281d5de89

Child:

15:40dd74bd48d1

--- a/main.cpp	Thu Nov 01 19:39:06 2018 +0000
+++ b/main.cpp	Thu Nov 01 20:44:47 2018 +0000
@@ -16,6 +16,8 @@
 DigitalIn button_clbrt_home(SW2);
 DigitalIn button_Demo(D2);
 DigitalIn button_Emg(D3);
+DigitalIn buttonx(D2); //x EMG replacement 
+DigitalIn buttony(D3); //y EMG replacement
 DigitalIn Fail_button(SW3);
 // Modserial
 MODSERIAL pc(USBTX, USBRX);
@@ -30,7 +32,8 @@
 // EMG input en start value of filtered EMG
 AnalogIn    emg1_raw( A0 );
 AnalogIn    emg2_raw( A1 );
-float threshold_EMG = 0.25;         // Threshold on 25 percent of the maximum EMG
+AnalogIn  potmeter1(PTC11);
+AnalogIn potmeter2(PTC10);
 
 // Declare timers and Tickers
 Timer       timer;                        // Timer for counting time in this state
@@ -41,10 +44,12 @@
 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_DEMO, FAILURE_MODE};
+enum states {WAITING, MOTOR_ANGLE_CLBRT, EMG_CLBRT, HOMING, WAITING4SIGNAL, MOVE_W_EMG, MOVE_W_KNOPJES, MOVE_W_DEMO, FAILURE_MODE};
 states currentState = WAITING;
 bool stateChanged = true;
 
+float threshold_EMG = 0.25;         // Threshold on 25 percent of the maximum EMG
+
 // Global variables
 char c;
 const float fs = 1/1024;
@@ -101,6 +106,14 @@
 double  point4y = 0;
 volatile int track = 1;
 
+// Motoraansturing met knopjes
+const double v=0.1; //moving speed of setpoint 
+
+double potwaarde1;
+double pot1;
+double potwaarde2;
+double pot2;
+
 // Determine demo setpoints
 const double stepsize1 = 0.15;
 const double stepsize2 = 0.25;
@@ -195,7 +208,7 @@
 }
 
 // PI controllers
-double PI_controller1(double error1)
+double PID_controller1(double error1)
 {
     static double error_integral1 = 0;
     static double error_prev1 = error1; // initialization with this value only done once!
@@ -222,7 +235,7 @@
     // Return
     return U1;      
 }
-double PI_controller2(double error2)
+double PID_controller2(double error2)
 {
     static double error_integral2 = 0;
     static double error_prev2 = error2; // initialization with this value only done once!
@@ -377,6 +390,38 @@
     return setpointy;
     
 }
+
+void determineknopjesset() {
+    setpointx = setpointx + dirx*sx*v;
+    setpointy = setpointy + diry*sy*v;
+    }
+    
+void motoraansturingknopjes()
+{
+    determineknopjesset();
+    q1_diff = makeAngleq1(setpointx, setpointy);
+    q2_diff = makeAngleq2(setpointx, setpointy);
+    //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;     
+}    
+
 void motoraansturingdemo()
 {    
     setpointx = determinedemosetx(setpointx, setpointy);
@@ -389,8 +434,8 @@
     theta1 = counts2angle1();  
     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);   
+    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
@@ -410,8 +455,8 @@
     theta1 = counts2angle1();  
     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);   
+    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
@@ -635,6 +680,57 @@
             stateChanged = true;
         }
         break;
+        
+        case MOVE_W_KNOPJES:
+        
+        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
+        if (stateChanged) {
+            motoraansturingknopjes();
+            stateChanged = true;
+        }
+        
+        potwaarde1 = potmeter1.read();  // Lees de potwaardes uit
+      
+        pot1 = potwaarde1*2 - 1;   // Scale van -1 tot 1 ipv. 0 tot 1
+        if (pot1 <  0)  {
+            dirx = -1;
+        }
+        else if (pot1 >= 0) {
+            dirx = 1;
+        }
+        
+        potwaarde2 = potmeter2.read();  // Lees de potwaardes uit
+      
+        pot2 = potwaarde2*2 - 1;   // Scale van -1 tot 1 ipv. 0 tot 1
+        if (pot2 <  0)  {
+            diry = -1;
+        }
+        else if (pot2 >= 0) {
+            diry = 1;
+        }
+        
+        sx = !buttonx.read(); //this has to be replaced with the input from the EMG, this then functions like a button
+        sy = !buttony.read(); //this has to be replaced with the input from the EMG, this then functions like a button  
+            
+        // State transition
+        if (button_clbrt_home == 0) 
+        {
+            currentState = HOMING;
+            stateChanged = true;
+            pc.printf("Moving home\n\r");
+        }
+        else if (Fail_button == 0)
+        {
+            currentState = FAILURE_MODE;
+            stateChanged = true;
+        }
+        
+        break; 
+        
  
 /**           
         case MOVE_W_EMG: