Diff: main.cpp

Revision:

1:afb820c6fc0d

Parent:

0:1b2c842eca42

Child:

2:3f67b4833256

--- a/main.cpp	Mon Oct 22 12:25:04 2018 +0000
+++ b/main.cpp	Tue Oct 30 14:26:15 2018 +0000
@@ -1,43 +1,151 @@
 #include "mbed.h"
+#include "QEI.h"
+#include "HIDScope.h"
+#include "MODSERIAL.h"
+
+PwmOut pwmpin1(D6);
+PwmOut pwmpin2(D5);
+AnalogIn potmeter1(A5);
+AnalogIn potmeter2(A4);
+DigitalIn button1(D2);
+DigitalIn button2(D3);
+DigitalOut directionpin1(D4);
+DigitalOut directionpin2(D7);
+QEI motor1(D13,D12,NC, 32);
+QEI motor2(D11,D10,NC, 32);
+
+DigitalIn kill_switch(SW2); //position has to be changed
+DigitalIn next_switch(SW3); //name and position should be replaced
+
+//for testing purposes
+DigitalOut ledred(LED_RED);
+DigitalOut ledgreen(LED_GREEN);
+DigitalOut ledblue(LED_BLUE);
+
+enum states{PositionCalibration, EmgCalibration, Movement, KILL};
+states CurrentState;
+MODSERIAL pc(USBTX, USBRX);
+//HIDScope scope(2);
+
+Ticker MotorsTicker;
+
+volatile float pwm_value1 = 0.0;
+volatile float pwm_value2 = 0.0;
 
-enum State(PositionCalibration, EmgCalibration, Movement, KILL);
+void positionCalibration() {
+    while(!button1){
+        directionpin2 = true;
+        pwm_value2 = 1.0f;
+    } 
+    pwm_value2 = 0.0f;
+    while(!button2){
+        directionpin2 = true;
+        pwm_value2 = 0.7f;
+        //wait(0.01f);
+    }
+    pwm_value2 = 0.0f;
+        
+    // pwm_value1 = potmeter1;
+    // pwm_value2 = potmeter2;
+    
+    if (!next_switch) {
+        CurrentState = EmgCalibration;
+        pc.printf("current state = EmgCalibration\n\r");
+    }
+}
+
+void emgCalibration() {
+    ledblue = !ledblue;
+    wait(0.5f);
+    
+    if (!next_switch) {
+        CurrentState = Movement;
+        pc.printf("current state = Movement\n\r");
+    }
+}
+
+void movement() {
+    
+}
+
+void move_motors() {
+    pwmpin1 = pwm_value1;
+    pwmpin2 = pwm_value2;
+}
 
 int main()
 {
-    State = PositionCalibration;
+    pc.baud(115200);
+    pc.printf(" ** program reset **\n\r");
+    pwmpin1.period_us(60);
+    pwmpin2.period_us(60);
+    directionpin1 = true;
+    directionpin2 = true;
+    ledred = true;
+    ledgreen = true;
+    ledblue = true;
+    
+    MotorsTicker.attach(&move_motors, 0.02f);
+    CurrentState = PositionCalibration;
+    pc.printf("current state = PositionCalibration\n\r");
     
     while (true) {
-        switch(States) {
+        switch(CurrentState) {
             case PositionCalibration:
-                if (!KillSwitch) {
-                    State = KILL;
-                    Break;
-                }
+                ledred = true;
+                ledgreen = false;
+                ledblue = true;
+                positionCalibration();
                 
-            Break;
-            
+                if (!kill_switch) {
+                    CurrentState = KILL;   
+                    pc.printf("current state = KILL\n\r"); 
+                }
+            break;
+                
             case EmgCalibration:
-                if (!KillSwitch) {
-                    State = KILL;
-                    Break;
+                ledred = true;
+                ledgreen = true;
+                // ledblue = false;
+                emgCalibration();
+                
+                if (!kill_switch) {
+                    CurrentState = KILL;   
+                    pc.printf("current state = KILL\n\r");  
                 }
-                
-            Break;
+            break;
             
             case Movement:
-                if (!KillSwitch) {
-                    State = KILL;
-                    Break;
+                ledred = true;
+                ledgreen = false;
+                ledblue = false;
+                movement();
+                
+                if (!kill_switch) {
+                    CurrentState = KILL; 
+                    pc.printf("current state = KILL\n\r");     
                 }
-                
-            Break;
+            break;
             
             case KILL:
-                turnoffmotors() //placeholder
-                flashsos() //placeholder
-                if (!KillSwitch){
-                    State = PositionCalibration;
+                bool u = true;
+                ledgreen = true;
+                ledblue = true;
+                ledred = false;
+                //turnoffmotors(); //placeholder
+                //flashsos() //placeholder 
+                wait(1.0f);
+                while(u) {
+                    if (!kill_switch) {
+                        u = false;
+                        ledred = true;
+                        wait(1.0f);
+                        CurrentState = PositionCalibration;
+                        pc.printf("current state = PositionCalibration\n\r");
+                    }
                 }
-            Break;           
+            break;  
+        }
+        wait(0.2f);         
     }
 }
\ No newline at end of file