main.cpp

00001 //============================================================================
00002 // Name        : piano_robot.cpp
00003 // Author      : 
00004 // Version     :
00005 // Copyright   : Your copyright notice
00006 // Description : Hello World in C++, Ansi-style
00007 //============================================================================
00008 
00009 #include <iostream>
00010 
00011 #include "mbed.h"
00012 #include "QEI.h"
00013 #include "read_filter_emg.h"
00014 #include "check_state.h"
00015 #include "check_state_change.h"
00016 #include "move_motor.h"
00017 
00018 AnalogIn analog_emg_left(A0);
00019 PinName pinLeft = A0;
00020 AnalogIn analog_emg_right(A1);
00021 PinName pinRight = A1;
00022 
00023 Ticker emg;
00024 
00025 //encoder for the horizontal motor
00026 QEI horizontal_encoder (PTC10, PTC11, NC, 624); // Pin for counting (analog in)
00027 
00028 //encoder for the keypress motor
00029 //QEI keypress_encoder (PTC10, PTC11, NC, 624); // Pin for counting (analog in)
00030 
00031 //initialise the variables for the horizontal motor 
00032 DigitalOut dir_horizontal(D4); 
00033 PwmOut pwm_horizontal(D5);
00034 double position_horizonal = horizontal_encoder.getPulses(); //actual position of horizontal movement motor
00035 double setpoint_horizonal = 1; //desired position, needs to be in pulses?
00036 double Kp_horizonal = 0; //controller gain
00037 double Ki_horizontal = 0; //set to zero
00038 
00039 //used to pass the value fo dir_horizontal by reference to the move motor funciton
00040 double dir_hor_ref = dir_horizontal;
00041 double pwm_hor_ref = pwm_horizontal;
00042 
00043 //initialise the variables for the keypress motor
00044 DigitalOut dir_keypress(D7); //direction
00045 PwmOut pwm_keypress(D6); //speed
00046 
00047 //change to keypress encoder!!
00048 double position_keypress = horizontal_encoder.getPulses(); //actual position of keypress movement motor
00049 
00050 double setpoint_keypress = 1; //desired position, always the same actually
00051 double Kp_keypress = 0; //controller gain, different from left and right
00052 double Ki_keypress = 0; //set to zero
00053 
00054 double dir_keypress_ref = dir_keypress;
00055 double pwm_keypress_ref = pwm_keypress;
00056         
00057 //initialised to the value of on threashold (value needed to turn on the system)
00058 //check_state_change alters the value depending on if the system has switched from on to off
00059 double threashold = 10;
00060 
00061 //tracks whether system is left,right,keypress,rest initialised to rest
00062 std::string state = "rest";
00063 
00064 //used to track how state from time to next time step
00065 std::string newState;
00066 
00067 //used to track the amount of time muscles are contracted, or newState changes over time
00068 int num_on_inputs = 0;
00069 
00070 //v1 and v2 are initialised to zero, they are updated in the emg.filter() function
00071 double v1_left = 0;
00072 double v2_left = 0;
00073 
00074 double v1_right = 0;
00075 double v2_right = 0;
00076 
00077 int main() {
00078     //create instance of left emg
00079     EMG emg_left(analog_emg_left, v1_left, v2_left);
00080     
00081     //EMG emg_left(pinLeft, v1_left, v2_left);
00082     
00083     //create instance of right emg
00084     EMG emg_right(analog_emg_right, v1_right, v2_right);
00085     
00086     //EMG emg_right(pinRight, v1_right, v2_right);
00087     
00088     /*THIS SECTION NEEDS TO BE CONTROLED USING TIMING*/
00089     while(true) 
00090     {
00091         //find value at current time
00092         emg_left.sample();
00093         //filter that value
00094         double input_left = emg_left.filter(emg_left.input_sample);
00095     
00096         //find value at current time
00097         emg_right.sample();
00098         //filter that value
00099         double input_right = emg_right.filter(emg_right.input_sample);
00100     
00101         //find which muscles are contracted above thresholds and return left,right,keypress or rest
00102         newState = check_state(input_left, input_right, threashold);
00103     
00104         //check whether new state = previous state and update num_on_inputs accordingly
00105         check_state_change(state, newState, num_on_inputs, threashold);
00106     
00107         //if num_on_inputs indicates same state for 0.25s move the necc. motor, if at rest just resets num_on_inputs
00108         //update(state, num_on_inputs);
00109         if (num_on_inputs == 250)
00110         {
00111             num_on_inputs = 0;
00112             if (state == "left")
00113             {
00114                 move_motor(dir_horizontal, pwm_horizontal, position_horizonal, setpoint_horizonal, Kp_horizonal, Ki_horizontal);
00115             }
00116             else if (state == "right")
00117             {
00118                 //same as left but setpoint is negative so it moves in the other direction
00119                 move_motor(dir_horizontal, pwm_horizontal, position_horizonal, -setpoint_horizonal, Kp_horizonal, Ki_horizontal);
00120             }
00121             else if (state == "keypress")
00122             {
00123                 move_motor(dir_keypress, pwm_keypress, position_keypress, setpoint_keypress, Kp_keypress, Ki_keypress);
00124                 //needs to move back to starting position so recall with opposite setpoint
00125                 move_motor(dir_keypress, pwm_keypress, position_keypress, setpoint_keypress, Kp_keypress, Ki_keypress);
00126             }
00127             //else rest do nothing
00128         }
00129     }
00130     return 0;
00131 }
00132