Diff: main.cpp

Revision:

15:9a1f34bc9958

Parent:

14:4cf17b10e504

Child:

16:696e9cbcc823

--- a/main.cpp	Mon Oct 07 13:36:38 2019 +0000
+++ b/main.cpp	Mon Oct 07 13:38:54 2019 +0000
@@ -143,181 +143,79 @@
 
 }
 
-<<<<<<< working copy
-void kinematics1()                                                  //
+
+void output()
 {
-    double length_1;
-    volatile double theta;
-    volatile double length_2;
+    return;
+}
 
-    class H_matrix                                                  //rows_columns (als je het niet ziet, gebruik dat de onderste rij 0 0 1 is)
-    {
-    public:
-        int h1_1_1 = 1;
-        int h1_1_2 = 0;
-        float h1_1_3 = sin(theta*PI/180)*(length_1+length_2);          //sinus en cosinus zijn standaard gedefinieërd in radians, maar theta is hier dus in graden
-        int h1_2_1 = 0;
-        int h1_2_2 = 1;
-        float h1_2_3 = cos(theta*PI/180)*(length_1+length_2);
-        int h1_3_1 = 0;
-        int h1_3_2 = 0;
-        int h1_3_3 = 1;
-    }
-    H_matrix H;
-
-    class Position_vector1                                          //positie vector gezien vanuit het uiteinde van de complete arm (coördinatiestelsel draait niet mee met de arm)
-    {
-    public:
-        float p1_1_1
-        float p1_2_1
-        float p1_3_1
-    }
-    class Position_vector2
-    {
-    public:
-        float p2_1_1 = h1_1_1*p1_1_1 + h1_1_2*p1_2_1 + h1_1_3*p1_3_1
-                       float p2_2_1 = h1_2_1*p1_1_1 + h1_2_2*p1_2_1 + h1_2_3*p1_3_1
-                                      float p2_3_1 = h1_3_1*p1_1_1 + h1_3_2*p1_2_1 + h1_3_3*p1_3_1
+void state_machine()
+{
+    //run current state
+    switch (current_state) {
+        case idle:
+            do_nothing();
+            break;
+        case failure:
+            failure();
+            break;
+        case cali_EMG:
+            cali_EMG();
+            break;
+        case cali_ENC:
+            cali_encoder();
+            break;
+        case moving_magnet_on:
+            moving_magnet_on();
+            break;
+        case moving_magnet_off:
+            moving_magnet_off();
+            break;
+        case homing:
+            homing();
+            break;
     }
 }
-=======
-    void motor_controller()
+
+void main_loop()
 {
-
-    motor1_direction = actuator.dir1;            // Zet de richting goed
-    motor1_pwm.write(actuator.duty_cycle1);             // Zet het op de snelheid van actuator.speed1
-
-    >>>>>>> merge rev
-
-    <<<<<<< working copy
-
-    void kinematics2() {                                                //
-        double length_1;
-        volatile double theta;
-        volatile double length_2;
-
-        class H_matrix2                                                     //rows_columns (als je het niet ziet, gebruik dat de onderste rij 0 0 1 is)
-        {
-        public:
-            int h2_1_1 = 1;
-            int h2_1_2 = 0;
-            float h2_1_3 = -sin(theta*PI/180)*(length_1+length_2);         //sinus en cosinus zijn standaard gedefinieërd in radians, maar theta is hier dus in graden
-            int h2_2_1 = 0;
-            int h2_2_2 = 1;
-            float h2_2_3 = -cos(theta*PI/180)*(length_1+length_2);
-            int h2_3_1 = 0;
-            int h2_3_2 = 0;
-            int h2_3_3 = 1;
-        }
-        H_matrix H;
-
-        class Position_vector3                                          //positie vector gezien vanuit het onderste draaipunt
-        {
-        public:
-            float p3_1_1
-            float p3_2_1
-            float p3_3_1
-        }
-        class Position_vector4
-        {
-        public:
-            float p4_1_1 = h2_1_1*p3_1_1 + h2_1_2*p3_2_1 + h2_1_3*p3_3_1
-                           float p4_2_1 = h2_2_1*p1_1_1 + h2_2_2*p1_2_1 + h2_2_3*p3_3_1
-                                          float p4_3_1 = h2_3_1*p1_1_1 + h2_3_2*p1_2_1 + h2_3_3*p3_3_1
-        }
-        H_matrix2 H2
-    }
-
-    void rotate_cw() {
-        if (state_changed) {
-            pc.printf("State changed to CW\r\n");
-            state_changed = false; //reset this so it wont print next loop
-        }
-        motor.dir1 = 1;             //todo: check if this is actually clockwise
-        motor.dir2 = 1;             //todo: check if this is actually clockwise
-
-        motor.pwm1 = x_input;       //nog niet echt de x, maar de rotatie, moet later met kinematics gefixt worden
-        motor.pwm2 = y_input;       // ook nog niet echt de y dus
-        //state guard
-        if (!state_changed && but1_pressed) { //state wasnt just changed, button 1 has been pressed -> change state
-            current_state = ccw;
-            state_changed = true;
-            but1_pressed = false; //reset this
-        }
-
-        =======
-            motor2_direction = actuator.dir2;
-        motor2_pwm.write(actuator.duty_cycle2);
-        return;
-        >>>>>>> merge rev
-    }
-
-    void output() {
-        return;
-    }
-
-    void state_machine() {
-        //run current state
-        switch (current_state) {
-            case idle:
-                do_nothing();
-                break;
-            case failure:
-                failure();
-                break;
-            case cali_EMG:
-                cali_EMG();
-                break;
-            case cali_ENC:
-                cali_encoder();
-                break;
-            case moving_magnet_on:
-                moving_magnet_on();
-                break;
-            case moving_magnet_off:
-                moving_magnet_off();
-                break;
-            case homing:
-                homing();
-                break;
-        }
-    }
-
-    void main_loop() {
-        measure_signals();
-        state_machine();
-        motor_controller();
-        output();
-    }
+    measure_signals();
+    state_machine();
+    motor_controller();
+    output();
+}
 
 //Helper functions, not directly called by the main_loop functions or
 //state machines
-    void but1_interrupt () {
-        but1_pressed = true;
-        pc.printf("Button 1 pressed \n\r");
-    }
+void but1_interrupt ()
+{
+    but1_pressed = true;
+    pc.printf("Button 1 pressed \n\r");
+}
 
-    void but2_interrupt () {
-        but2_pressed = true;
-        pc.printf("Button 2 pressed \n\r");
-    }
-
-    int main() {
-        pc.baud(115200);
-        pc.printf("Executing main()... \r\n");
-        current_state = idle;
+void but2_interrupt ()
+{
+    but2_pressed = true;
+    pc.printf("Button 2 pressed \n\r");
+}
 
-        motor2_pwm.period(1/160000f);   // 1/frequency van waarop hij draait
-        motor1_pwm.period(1/160000f);   // 1/frequency van waarop hij draait
+int main()
+{
+    pc.baud(115200);
+    pc.printf("Executing main()... \r\n");
+    current_state = idle;
 
-        actuator.dir1 = 0;
-        actuator.dir2 = 0;
+    motor2_pwm.period(1/160000f);   // 1/frequency van waarop hij draait
+    motor1_pwm.period(1/160000f);   // 1/frequency van waarop hij draait
 
-        actuator.magnet = false;
+    actuator.dir1 = 0;
+    actuator.dir2 = 0;
+
+    actuator.magnet = false;
 
-        but1.fall(&but1_interrupt);
-        but2.fall(&but2_interrupt);
-        loop_ticker.attach(&main_loop, 0.001f); //main loop at 1kHz
-        pc.printf("Main_loop is running\n\r");
-        while (true) {}
-    }
\ No newline at end of file
+    but1.fall(&but1_interrupt);
+    but2.fall(&but2_interrupt);
+    loop_ticker.attach(&main_loop, 0.001f); //main loop at 1kHz
+    pc.printf("Main_loop is running\n\r");
+    while (true) {}
+}
\ No newline at end of file