Diff: main.cpp

Revision:

5:aa8b5d5e632f

Parent:

4:36e32ddf2443

Child:

6:354a6509405f

--- a/main.cpp	Wed Oct 02 12:49:05 2019 +0000
+++ b/main.cpp	Thu Oct 03 10:53:17 2019 +0000
@@ -5,21 +5,116 @@
 
 MODSERIAL pc(USBTX, USBRX);         //verbinden met pc
 DigitalOut motor2_direction(D4);    //verbinden met motor 2 op board (altijd d4)
-
+FastPWM motor2_pwm(D5);             //verbinden met motor 2 pwm (altijd d5)
+Ticker loop_ticker;                 //used in main()
+InterruptIn button(D10);            //knop op birorobotics shield
 QEI encoder (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder gebruiken
 
-int enc_count;
-int prev_count = 0;
+//variables
+enum States {idle, cw, ccw, end, failure};
+States current_state;
+class motor_state {
+    public:
+    float pwm1; //pwm of 1st motor
+    float pwm2; //pwm of 2nd motor
+    int dir1; //direction of 1st motor
+    int dir2; //direction of 2nd motor
+};
+motor_state motor;
+
+bool state_changed = false;
+volatile bool button_pressed = false;
+
+void measure_signals() {return;}
+
+void do_nothing() {
+    if (button_pressed) {
+        current_state = cw;
+        state_changed = true;
+        pc.printf("Changed state from idle to cw\r\n");
+        button_pressed = false;
+    }
+}
+void rotate_cw() {
+    if (state_changed) {
+        pc.printf("State changed to CW\r\n");
+        state_changed = false;
+    }
+    motor.dir2 = 1;
+    motor.pwm2 = 0.5f;
+    if (!state_changed && button_pressed) { //state niet veranderd, button gepressd -> state verandert
+        current_state = ccw;
+        state_changed = true;
+        button_pressed = false;
+    }
+}
+
+void rotate_ccw() {
+    if (state_changed) {
+        pc.printf("State changed to CCW\r\n");
+        state_changed = false;
+    }
+    motor.dir2 = 0;
+    motor.pwm2 = 0.5f;
+    if (!state_changed && button_pressed) { //state niet veranderd, button gepressd -> state verandert
+        current_state = cw;
+        state_changed = true;
+        button_pressed = false;
+    }
+}
+        
+void motor_controller() {return;}
+void output() {return;}
+
+void button_interrupt () {
+    button_pressed = true;
+}
+
+void state_machine() {
+    
+    //run current state
+    switch (current_state) {
+        case idle:
+            do_nothing();
+            break;
+        case cw:
+            rotate_cw();
+            break;
+        case ccw:
+            rotate_ccw();
+            break;
+        case end:
+            break;
+        case failure:
+            break;
+    }        
+}
+
+void main_loop() {
+    measure_signals();
+    state_machine();
+    motor_controller();
+    output();
+}
 
 int main() {
     pc.baud(115200);
-    pc.printf("initializing\r\n");
+    pc.printf("Executing main()\r\n");
+    current_state = idle;
     
-    while (true) {
-        enc_count = encoder.getPulses(); //kijkt op welke stand hij nu staat
-        pc.printf("%f\r\n", float(enc_count-prev_count)*360/(8400*0.025)); //verschil met vorige stap in graden per stap gedeeld door tijd = snelheid
-        prev_count = enc_count;
-        wait_ms(25);
-    }
+    motor.pwm1 = 0;
+    motor.pwm2 = 0;
+    motor.dir1 = 0;
+    motor.dir2 = 0;
+    button.fall(&button_interrupt);
+    loop_ticker.attach(&main_loop, 0.001f); //main loop at 1kHz
+    
+    while (true) {}
+    //while (true) {
+    //    enc_count = encoder.getPulses(); //kijkt op welke stand hij nu staat
+    //    pc.printf("%f\r\n", float(enc_count-prev_count)*360/(8400*0.025)); //verschil met vorige stap in graden per stap gedeeld door tijd = snelheid
+    //    prev_count = enc_count;
+    //    wait_ms(25);
+    //}
 }
         
\ No newline at end of file