Diff: main.cpp

Revision:

18:a584af8b4cfb

Parent:

17:615c5d8b3710

--- a/main.cpp	Fri Oct 11 09:51:35 2019 +0000
+++ b/main.cpp	Fri Oct 11 11:05:25 2019 +0000
@@ -2,79 +2,57 @@
 #include "MODSERIAL.h"
 #include "FastPWM.h"
 #include "QEI.h"
-#include "HIDScope.h"
-#include "BiQuad.h"
 #define PI 3.14159265
 
 Serial pc(USBTX, USBRX);            //connect to pc
-HIDScope scope(3);                  //HIDScope instance
 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 on biorobotics shield
 AnalogIn Pot2(A0);                  //pot 2 on biorobotics shield
+*/ 
+
+AnalogIn vrx(A0);                   //Joystick_x
+AnalogIn vry(A1);                   //Joystick_y
+
 InterruptIn but1(D10);              //debounced button on biorobotics shield
 InterruptIn but2(D9);               //debounced button on biorobotics shield
-AnalogIn EMG1(A2);
-AnalogIn EMG2(A3);
-
-void check_failure();
 
-QEI enc1 (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder 1 gebruiken
-QEI enc2 (D1,  D2,  NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken
+QEI encoder1 (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder 1 gebruiken
+QEI encoder2 (D1,  D2,  NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken
 
-BiQuad bq1 (0.881889334678067,    -1.76377866935613,    0.8818893346780671,   -1.77069673005903, 0.797707797506027);
-BiQuad bq2 (0.000198358203463849, 0.000396716406927699, 0.000198358203463849, -1.96262073248799, 0.963423352820821);
+float Joystick_x = 0.0;
+float Joystick_y = 0.0;
 
 //variables
-enum States {s_idle, s_cali_EMG, s_cali_enc, s_moving_magnet_off, s_moving_magnet_on, s_homing, s_failure};
-States state;                       //using the States enum
+enum States {idle, calib_EMG, calib_enc, moving_magnet_offf, moving_magnet_onn, home, fail};
+States current_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
     bool magnet;                    //state of the magnet
-} actuator;
+} actuators;
 
 struct EMG_params {
-    float max;  //params of the emg, tbd during calibration
-    float min;
-} EMG_values;
+    float idk;  //params of the emg, tbd during calibration
+} emg_values;
 
-int enc1_zero;  //the zero position of the encoders, to be determined from the
-int enc2_zero;  //encoder calibration
-int EMG1_filtered;
-int EMG2_filtered;
-int enc1_value;
-int enc2_value;
+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;
-volatile bool failure_occurred = false;
 float pot_1;                //used to keep track of the potentiometer values
 float pot_2;
-bool enc_has_been_calibrated;
-bool EMG_has_been_calibrated;
-bool button1_pressed;
-bool button2_pressed;
 
-void do_nothing()
-
-/*
-    Idle state. Used in the beginning, before the calibration states.
-*/
-{
-    if (button1_pressed) {
-        state_changed = true;
-        state = s_cali_enc;
-        button1_pressed = false;
-    }
-}
 
 void failure()
 /*
@@ -89,7 +67,7 @@
 
 void cali_EMG()
 /*
-    Calibration of the EMG. Values determined during calibration should be
+    Calibratioin of the EMG. Values determined during calibration should be
     added to the EMG_params instance.
 */
 {
@@ -98,7 +76,6 @@
         state_changed = false;
     }
 }
-
 void cali_enc()
 /*
     Calibration of the encoder. The encoder should be moved to the lowest
@@ -110,17 +87,7 @@
         pc.printf("Started encoder calibration\r\n");
         state_changed = false;
     }
-    if (button1_pressed) {
-        enc1_zero = enc1_value;
-        enc2_zero = enc2_value;
-        enc_has_been_calibrated = true;
-        button1_pressed = false;
-        state = s_moving_magnet_off;
-        state_changed = true;
-
-    }
 }
-
 void moving_magnet_off()
 /*
     Moving with the magnet disabled. This is the part from the home position
@@ -131,8 +98,8 @@
         pc.printf("Moving without magnet\r\n");
         state_changed = false;
     }
+    return;
 }
-
 void moving_magnet_on()
 /*
     Moving with the magnet enabled. This is the part of the movement from the
@@ -157,84 +124,84 @@
     return;
 }
 
+// the funtions needed to run the program
 void measure_signals()
 {
-    float EMG1_raw;
-    float EMG2_raw;
-    enc1_value = enc1.getPulses();
-    enc2_value = enc2.getPulses();
-    if (enc_has_been_calibrated) {
-        enc1_value -= enc1_zero;
-        enc2_value -= enc2_zero;
-    }
-    EMG1_raw = EMG1.read();
-    EMG2_raw = EMG2.read();
+    Joystick_x = (vrx.read()*100-50)/50;
+    Joystick_y = (vry.read()*100-50)/50;
+    pc.printf("X-coordinate = %.2f | Y-coordinate = %.2f \n\r", Joystick_x, Joystick_y);
+
+    return;
 }
 
-void sample()
+void do_nothing()
 {
-    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
-    float emg0_value = EMG1.read(); 
-    float emg1_value = EMG2.read();
-    
-    //double filter_value = bqc.step(emg1_value);
-    float filter_value = fabs(bq2.step(fabs(bq1.step(emg0_value - emg1_value))));
-    if (filter_value > EMG_values.max) {
-        EMG_values.max = filter_value;
-        }
-    if (EMG_values.min > filter_value) {
-        EMG_values.min = filter_value;
-        }
-    
-    filter_value = filter_value-EMG_values.min;
-    filter_value = filter_value/(EMG_values.max-EMG_values.min);
-    
-    scope.set(0, EMG1.read() );
-    scope.set(1, EMG2.read() );
-    scope.set(2, filter_value);
-    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
-    *  Ensure that enough channels are available (HIDScope scope( 2 ))
-    *  Finally, send all channels to the PC at once */
-    scope.send();
-    /* To indicate that the function is working, the LED is toggled */
+    actuators.duty_cycle1 = 0;
+    actuators.duty_cycle2 = 0;
+
+    /*
+    //state guard
+    if (but1_pressed) { //this moves the program from the idle to cw state
+        current_state = cw;
+        state_changed = true; //to show next state it can initialize
+        pc.printf("Changed state from idle to cw\r\n");
+        but1_pressed = false; //reset button 1
+    }
+    */
 }
 
-void motor_controller() {
-    
-}
 
 void output()
 {
-    motor1_direction = actuator.dir1;
-    motor2_direction = actuator.dir2;
-    motor1_pwm.write(actuator.duty_cycle1);
-    motor2_pwm.write(actuator.duty_cycle2);
+    
+    if (Joystick_x < 0){
+        motor2_pwm.write(fabs(Joystick_x));   // 1/frequency van waarop hij draait
+        actuators.dir2 = 0;
+        motor2_dir = motor.dir2;
+    }
+    else {
+        motor2_pwm.write(Joystick_x);   // 1/frequency van waarop hij draait
+        motor.dir2 = 1;
+        motor2_dir = motor.dir2;
+    }
+    if (Joystick_y < 0){
+        motor1_pwm.write(fabs(Joystick_x));   // 1/frequency van waarop hij draait
+        motor.dir1 = 0;
+        motor1_dir = motor.dir1;
+    }
+    else {
+        motor1_pwm.write(Joystick_x);   // 1/frequency van waarop hij draait
+        motor.dir1 = 1;
+        motor1_dir = motor.dir1;
+    }
+    
+    
+    return;
 }
 
 void state_machine()
 {
-    check_failure(); //check for an error in the last loop before state machine
     //run current state
-    switch (state) {
-        case s_idle:
+    switch (current_state) {
+        case idle:
             do_nothing();
             break;
-        case s_failure:
+        case fail:
             failure();
             break;
-        case s_cali_EMG:
+        case calib_EMG:
             cali_EMG();
             break;
-        case s_cali_enc:
-            cali_enc();
+        case calib_ENC:
+            cali_encoder();
             break;
-        case s_moving_magnet_on:
+        case moving_magnet_onn:
             moving_magnet_on();
             break;
-        case s_moving_magnet_off:
+        case moving_magnet_offf:
             moving_magnet_off();
             break;
-        case s_homing:
+        case homing:
             homing();
             break;
     }
@@ -250,61 +217,35 @@
 
 //Helper functions, not directly called by the main_loop functions or
 //state machines
-void check_failure()
+void but1_interrupt ()
 {
-    state = s_failure;
-    state_changed = true;
-}
-
-void but1_interrupt()
-{
-    if(but2.read()) {//both buttons are pressed
-        failure_occurred = true;
-    }
     but1_pressed = true;
     pc.printf("Button 1 pressed \n\r");
 }
 
-void but2_interrupt()
+void but2_interrupt ()
 {
-    if(but1.read()) {//both buttons are pressed
-        failure_occurred = true;
-    }
     but2_pressed = true;
     pc.printf("Button 2 pressed \n\r");
 }
 
-int schmitt_trigger(float i) 
-{
-    int speed;
-    speed = -1; //default value, this means the state should not change
-    if (i >  0/14 && i <  2/14) {speed = 0;}
-    if (i >  3/14 && i <  5/14) {speed = 1;}
-    if (i >  6/14 && i <  8/14) {speed = 2;}
-    if (i >  9/14 && i < 11/14) {speed = 3;}
-    if (i > 12/14 && i < 14/14) {speed = 4;}    
-    return speed;
-}
-
 int main()
 {
     pc.baud(115200);
     pc.printf("Executing main()... \r\n");
-    state = s_idle;
+    current_state = idle;
 
-    motor2_pwm.period(1/160000);   // 1/frequency van waarop hij draait
-    motor1_pwm.period(1/160000);   // 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;
+    actuators.dir1 = 0;
+    actuators.dir2 = 0;
 
-    actuator.magnet = false;
+    actuators.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) {
-        wait(0.1f);
-    }
+    while (true) {}
 }
\ No newline at end of file