Biorobotics
/
piano_robot
newest version,
main.cpp
- Committer:
- roosbulthuis
- Date:
- 2015-10-26
- Revision:
- 3:11c2175b4478
- Parent:
- 1:5c1fc4e9401a
File content as of revision 3:11c2175b4478:
//============================================================================ // Name : piano_robot.cpp // Author : // Version : // Copyright : Your copyright notice // Description : Hello World in C++, Ansi-style //============================================================================ #include <iostream> #include "mbed.h" #include "QEI.h" #include "read_filter_emg.h" #include "check_state.h" #include "check_state_change.h" #include "move_motor.h" AnalogIn analog_emg_left(A0); PinName pinLeft = A0; AnalogIn analog_emg_right(A1); PinName pinRight = A1; Ticker emg; //encoder for the horizontal motor QEI horizontal_encoder (PTC10, PTC11, NC, 624); // Pin for counting (analog in) //encoder for the keypress motor //QEI keypress_encoder (PTC10, PTC11, NC, 624); // Pin for counting (analog in) //initialise the variables for the horizontal motor DigitalOut dir_horizontal(D4); PwmOut pwm_horizontal(D5); double position_horizonal = horizontal_encoder.getPulses(); //actual position of horizontal movement motor double setpoint_horizonal = 1; //desired position, needs to be in pulses? double Kp_horizonal = 0; //controller gain double Ki_horizontal = 0; //set to zero //used to pass the value fo dir_horizontal by reference to the move motor funciton double dir_hor_ref = dir_horizontal; double pwm_hor_ref = pwm_horizontal; //initialise the variables for the keypress motor DigitalOut dir_keypress(D7); //direction PwmOut pwm_keypress(D6); //speed //change to keypress encoder!! double position_keypress = horizontal_encoder.getPulses(); //actual position of keypress movement motor double setpoint_keypress = 1; //desired position, always the same actually double Kp_keypress = 0; //controller gain, different from left and right double Ki_keypress = 0; //set to zero double dir_keypress_ref = dir_keypress; double pwm_keypress_ref = pwm_keypress; //initialised to the value of on threashold (value needed to turn on the system) //check_state_change alters the value depending on if the system has switched from on to off double threashold = 10; //tracks whether system is left,right,keypress,rest initialised to rest std::string state = "rest"; //used to track how state from time to next time step std::string newState; //used to track the amount of time muscles are contracted, or newState changes over time int num_on_inputs = 0; //v1 and v2 are initialised to zero, they are updated in the emg.filter() function double v1_left = 0; double v2_left = 0; double v1_right = 0; double v2_right = 0; int main() { //create instance of left emg EMG emg_left(analog_emg_left, v1_left, v2_left); //EMG emg_left(pinLeft, v1_left, v2_left); //create instance of right emg EMG emg_right(analog_emg_right, v1_right, v2_right); //EMG emg_right(pinRight, v1_right, v2_right); /*THIS SECTION NEEDS TO BE CONTROLED USING TIMING*/ while(true) { //find value at current time emg_left.sample(); //filter that value double input_left = emg_left.filter(emg_left.input_sample); //find value at current time emg_right.sample(); //filter that value double input_right = emg_right.filter(emg_right.input_sample); //find which muscles are contracted above thresholds and return left,right,keypress or rest newState = check_state(input_left, input_right, threashold); //check whether new state = previous state and update num_on_inputs accordingly check_state_change(state, newState, num_on_inputs, threashold); //if num_on_inputs indicates same state for 0.25s move the necc. motor, if at rest just resets num_on_inputs //update(state, num_on_inputs); if (num_on_inputs == 250) { num_on_inputs = 0; if (state == "left") { move_motor(dir_horizontal, pwm_horizontal, position_horizonal, setpoint_horizonal, Kp_horizonal, Ki_horizontal); } else if (state == "right") { //same as left but setpoint is negative so it moves in the other direction move_motor(dir_horizontal, pwm_horizontal, position_horizonal, -setpoint_horizonal, Kp_horizonal, Ki_horizontal); } else if (state == "keypress") { move_motor(dir_keypress, pwm_keypress, position_keypress, setpoint_keypress, Kp_keypress, Ki_keypress); //needs to move back to starting position so recall with opposite setpoint move_motor(dir_keypress, pwm_keypress, position_keypress, setpoint_keypress, Kp_keypress, Ki_keypress); } //else rest do nothing } } return 0; }