Diff: main.cpp

Revision:

12:88cbc65f2563

Parent:

11:f83e3bf7febf

Child:

14:4cf17b10e504

--- a/main.cpp	Mon Oct 07 08:38:07 2019 +0000
+++ b/main.cpp	Mon Oct 07 13:17:35 2019 +0000
@@ -3,111 +3,140 @@
 #include "FastPWM.h"
 #include "QEI.h"
 
-Serial 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)
-DigitalOut motor1_direction(D6);    //verbinden met motor 2 op board (altijd d6)
-FastPWM motor1_pwm(D7);             //verbinden met motor 2 pwm (altijd d7)
+Serial pc(USBTX, USBRX);            //connect to pc
+DigitalOut motor1_direction(D4);    //rotation motor 1 on shield (always D6)
+FastPWM motor1_pwm(D5);             //pwm 1 on shield (always D7)
+DigitalOut motor2_direction(D7);    //rotation motor 2 on shield (always D4)
+FastPWM motor2_pwm(D6);             //pwm 2 on shield (always D5)
 Ticker loop_ticker;                 //used in main()
-AnalogIn Pot1(A1);                  //pot 1 op biorobotics shield
-AnalogIn Pot2(A0);                  //pot 2 op biorobotics shield
-AnalogIn joystick_x(A2);            //input joystick
-AnalogIn joystick_y(A3);            //input joystick
-InterruptIn but1(D10);              //knop 1 op birorobotics shield
-InterruptIn but2(D9);               //knop 1 op birorobotics shield
+AnalogIn Pot1(A1);                  //pot 1 on biorobotics shield
+AnalogIn Pot2(A0);                  //pot 2 on biorobotics shield
+InterruptIn but1(D10);              //debounced button on biorobotics shield
+InterruptIn but2(D9);               //debounced button on biorobotics shield
+
 QEI encoder1 (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder 1 gebruiken
 QEI encoder2 (D1,  D2,  NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken
 
 
 //variables
-enum States {idle, cw, ccw, end, failure};
-States current_state;               //Definieren van de current_state met als keuzes Idle, cw, ccw, end en failure
-
-class motor_state {                 // Een emmer met variabelen die je hierna motor gaat noemen.
-    public:                         //variabelen kunnen worden gebruikt door externe functies
-    float pwm1;                     //pwm of 1st motor
-    float pwm2;                     //pwm of 2nd motor
+enum States {idle, cali_EMG, cali_enc, moving_magnet_off, moving_magnet_on, homing, failure};
+States state;                       //using the States enum
+struct actuator_state
+{
+    float duty_cycle1;              //pwm of 1st motor
+    float duty_cycle2;              //pwm of 2nd motor
     int dir1;                       //direction of 1st motor
     int dir2;                       //direction of 2nd motor
-};
-motor_state motor;                  //Een emmer met variabelen met motor.iets als naam bijv. motor.speed1
+    bool magnet;                    //state of the magnet
+} actuators;
+
+struct EMG_params
+{
+    float idk;  //params of the emg, tbd during calibration
+} emg_values;
 
-bool state_changed = false;         // wordt gebruikt in de state functies, om te kijken of de state net begint
+//initializing enums and classes
+
+
+int enc_zero;   //the zero position of the encoders, to be determined from the 
+                //encoder calibration
+
+//variables used throughout the program
+bool state_changed = false; //used to see if the state is "starting"
 volatile bool but1_pressed = false;
 volatile bool but2_pressed = false;
-float pot_1;
+float pot_1;                //used to keep track of the potentiometer values
 float pot_2;
-float x_input;
-float y_input;
+
+void do_nothing()
 
 /*
-float x_movement = 0;
-float y_movement = 0;
-float angle = 0;
-float extention = 0;
+    Idle state. Used in the beginning, before the calibration states.
+*/
+{}
+
+void failure()
+/*
+    Failure mode. This should execute when button 2 is pressed during operation.
+*/
+{
+    if (state_changed) {
+        pc.printf("Something went wrong!\r\n");
+        state_changed = false;
+    }
+}
 
-void calibration() {                //kalibratie van de sensoren (de nulstand invoeren)
-    letter = pc.getc()              // krijg de letter vanuit de keyboard
-    while (letter == w){            // of hier een while loop moet ja of nee, geen idee, lijkt me beter dan een if naar mijn mening. Eigenlijk wil je iets op de rising en falling edge hebben om zo het begin en het eind erin te kunne zetten. 
-        y_movement = 1;
+void cali_EMG()
+/*
+    Calibratioin of the EMG. Values determined during calibration should be
+    added to the EMG_params instance.
+*/
+{
+    if (state_changed) {
+        pc.printf("Started EMG calibration\r\n");
+        state_changed = false;
     }
-    while (letter == a) {
-        x_movement = -1;
-    }
-    while (letter == s){
-        y_movement = -1;
+}
+void cali_enc()
+/*
+    Calibration of the encoder. The encoder should be moved to the lowest 
+    position for the linear stage and the most upright postition for the 
+    rotating stage.
+*/
+{
+    if (state_changed) {
+        pc.printf("Started encoder calibration\r\n");
+        state_changed = false;
     }
-    while (letter == d) {
-        x_movement = 1;
+}
+void moving_magnet_off()
+/*
+    Moving with the magnet disabled. This is the part from the home position 
+    towards the storage of chips.
+*/
+{
+    if (state_changed) {
+        pc.printf("Moving without magnet\r\n");
+        state_changed = false;
     }
-    robot_kinematics()                  // hoe dat dit precies moet geen idee. Dit lijkt me een goed begin of iig een begin
-    x_movement = 0;
-    y_movement = 0;    
-    
-    
-
+    return;
 }
-void robot_kinematics() {           //hoe de motoren moeten draaien als er een x of y richting als input is. 
-    [motor.dir1, motor.dir2; motor.speed1, motor.speed2] = H^-1 *[x_movement, y_movement; angle]        // Zoiets
-    
-    
+void moving_magnet_on()
+/*
+    Moving with the magnet enabled. This is the part of the movement from the 
+    chip holder to the top of the playing board.
+*/
+{
+    if (state_changed) {
+        pc.printf("Moving with magnet\r\n");
+        state_changed = false;
+    }
+    return;
 }
+void homing()
+/*
+    Dropping the chip and moving towards the rest position. 
 */
+{
+    if (state_changed) {
+        pc.printf("Started homing");
+        state_changed = false;
+    }
+    return;
+}
 
 // the funtions needed to run the program
-void measure_signals() {
-    x_input = joystick_x.read();
-    y_input = joystick_y.read();
+void measure_signals()
+{
+    return;
+}
 
-    /*
-    letter = pc.getc()          // krijg de letter vanuit de keyboard
-    if (letter == w){
-        y_movement = 1;
-    }
-    if (letter == a) {
-        x_movement = -1;
-    }
-    if (letter == s){
-        y_movement = -1;
-    }
-    if (letter == d) {
-        x_movement = 1;
-    }    
-    
-    angle = (encoder1.getPulses()-angle_0)/8400*360; //kijkt op welke stand encoder 1 nu staat (dat hoort dus de basis motor te zijn!) /8400 signalen per rotatie * 360 graden
-    extention = encoder2.getPulses()-extention_0/8400*8*5.05; //kijkt hoever de arm is uitgeschoven (/8400 per rotatie *8mm per rotatie aan verschuiving * 5.05 de gear ratio) 
-    
-    
-    */
+void do_nothing()
+{
+    actuator.duty_cycle1 = 0;
+    actuator.duty_cycle2 = 0;
 
-    return;
-    }
 
-void do_nothing() {
-    motor.speed1 = 0;
-    motor.speed2 = 0;
-    
-    
     //state guard
     if (but1_pressed) { //this moves the program from the idle to cw state
         current_state = cw;
@@ -115,119 +144,93 @@
         pc.printf("Changed state from idle to cw\r\n");
         but1_pressed = false; //reset button 1
     }
-    
+
 }
 
+void motor_controller()
+{
+
+    motor1_direction = actuator.dir1;            // Zet de richting goed
+    motor1_pwm.write(actuator.duty_cycle1);             // Zet het op de snelheid van actuator.speed1
+
 
-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;
+}
+
+void output()
+{
+    return;
 }
- 
-void rotate_ccw() {
-    //similar to rotate_cw()
-    if (state_changed) {
-        pc.printf("State changed to CCW\r\n");
-        state_changed = false;
-    }
-    motor.dir1 = 0;
-    motor.dir2 = 0;
-    
-    motor.pwm1 = x_input;       //nog niet echt de x
-    motor.pwm2 = y_input;       //nog niet echt de y
-    
-    //state guard
-    if (!state_changed && but1_pressed) { //state niet veranderd, button 1 gepressd -> state verandert
-        current_state = cw;
-        state_changed = true;
-        but1_pressed = false;
+
+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() {
 
-    motor1_direction = motor.dir1;              // Zet de richting goed
-    motor1_pwm.write(motor.pwm1);             // Zet het op de snelheid van motor.speed1
-    
-
-    motor2_direction = motor.dir2;
-    motor2_pwm.write(motor.pwm2);             
-    return;
-}
-    
-void output() {return;}
-
-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");
-}
-
-void state_machine() {
-    
-    if (but2_pressed){              // Is dit de goede locatie hiervoor?
-        current_state = idle;
-        but2_pressed = false;
-        pc.printf("Do_nothing happened due to pressing button 2 \n\r");   
-    }
-    
-    //run current state
-    switch (current_state) {
-        case idle:                  // hoezo de :?
-            do_nothing();
-            break;
-        case cw:
-            rotate_cw();
-            break;
-        case ccw:
-            rotate_ccw();
-            break;
-        case end:
-            break;
-        case failure:
-            break;
-    }        
-}
-
-    
-void main_loop() {
+void main_loop()
+{
     measure_signals();
     state_machine();
     motor_controller();
     output();
 }
 
-int main() {
+//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 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;
     
-    motor.dir1 = 0;
-    motor.dir2 = 0;
-    motor2_pwm.period(1/160000f);              // 1/frequency van waarop hij draait
-    motor1_pwm.period(1/160000f);              // 1/frequency van waarop hij draait
+    motor2_pwm.period(1/160000f);   // 1/frequency van waarop hij draait
+    motor1_pwm.period(1/160000f);   // 1/frequency van waarop hij draait
+
+    actuator.dir1 = 0;
+    actuator.dir2 = 0;
+    
+    actuator.magnet = false;
     
     but1.fall(&but1_interrupt);
     but2.fall(&but2_interrupt);
-    loop_ticker.attach(&main_loop, 0.1f); //main loop at 1kHz (niet per tiende seconde?)
-    pc.printf("Main_loop is running\n \r");
+    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
+}
\ No newline at end of file