Diff: main.cpp

Revision:

14:dc89250ebc52

Parent:

13:ec4708dab45d

Child:

15:c4799ad02cdc

--- a/main.cpp	Fri Oct 11 12:58:49 2019 +0000
+++ b/main.cpp	Mon Oct 14 13:42:38 2019 +0000
@@ -10,7 +10,7 @@
 AnalogIn ain1(A3);
 DigitalOut dir2(D4);
 DigitalOut dir1(D7);
-//DigitalOut pwm(D5);
+
 
 
 Ticker ticktick;
@@ -22,13 +22,16 @@
 
 Ticker      sample_timer;
 HIDScope    scope( 2 );
-DigitalOut  led(LED1);
+DigitalOut  ledred(LED_RED);
+DigitalOut  ledblue(LED_BLUE);
+DigitalOut  ledgreen(LED_GREEN);
 volatile float P;
+enum states{Waiting,Position_calibration,EMG_calibration,Homing,Operating,Demo,Failure};
+states currentState=Operating;
+InterruptIn err(SW2);
 
-/** Sample function
- * this function samples the emg and sends it to HIDScope
- **/
-void sample()
+
+void read_emg()
 {
     static int count=0;
     static float RMS_value=0;
@@ -53,9 +56,12 @@
     *  Finally, send all channels to the PC at once */
     scope.send();
     /* To indicate that the function is working, the LED is toggled */
-    led = !led;
+    ledred=1;
+    ledgreen=0;
+    ledblue=1;
 }
-void setPWM(void)
+
+void set_PWM(void)
 {
    float Q;
    if (7*P>1)
@@ -70,14 +76,42 @@
    motor2_pwm.write(ain1.read());
 }
 
+void sample()
+{
+    switch(currentState)
+    {
+        case Operating:
+            read_emg();
+            set_PWM();
+            break;
+        case Failure:
+            ledred=0;
+            ledgreen=1;
+            ledblue=1;
+            break;
+        default:
+            ledred=1;
+            ledgreen=1;
+            ledblue=0;
+        break;
+    }
+}
+
+void error_occur()
+{
+    currentState=Failure;
+}
+
 int main()
 {
+    pc.baud(115200);
+    pc.printf("Starting...");
+    ledred=0;
     sample_timer.attach(&sample, 0.002);
-    ticktick.attach(setPWM,0.1);
+    err.fall(error_occur);
     int frequency_pwm=10000;
     motor1_pwm.period(1.0/frequency_pwm);
     motor2_pwm.period(1.0/frequency_pwm);
-    pc.printf("Starting...");
     
     while (true) {
         wait(10);