Biorobotics / Mbed 2 deprecated piano_robot

newest version,

Dependencies:   QEI mbed

main.cpp@0:fc6fa085d591, 2015-10-23 (annotated)

Committer:
NickDGreg
Date:
Fri Oct 23 13:14:57 2015 +0000
Revision:
0:fc6fa085d591
Child:
1:5c1fc4e9401a
move_motor compiles, working on filter.  Cannot pass AnalogIn as input, says no default constructor. Cannot pass as pinName as analogIn declared in constructor cannot be seen  by methods; ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NickDGreg 0:fc6fa085d591 1 //============================================================================
NickDGreg 0:fc6fa085d591 2 // Name : piano_robot.cpp
NickDGreg 0:fc6fa085d591 3 // Author :
NickDGreg 0:fc6fa085d591 4 // Version :
NickDGreg 0:fc6fa085d591 5 // Copyright : Your copyright notice
NickDGreg 0:fc6fa085d591 6 // Description : Hello World in C++, Ansi-style
NickDGreg 0:fc6fa085d591 7 //============================================================================
NickDGreg 0:fc6fa085d591 8
NickDGreg 0:fc6fa085d591 9 #include <iostream>
NickDGreg 0:fc6fa085d591 10
NickDGreg 0:fc6fa085d591 11 #include "mbed.h"
NickDGreg 0:fc6fa085d591 12 #include "QEI.h"
NickDGreg 0:fc6fa085d591 13 #include "read_filter_emg.h"
NickDGreg 0:fc6fa085d591 14 #include "check_state.h"
NickDGreg 0:fc6fa085d591 15 #include "check_state_change.h"
NickDGreg 0:fc6fa085d591 16 #include "move_motor.h"
NickDGreg 0:fc6fa085d591 17
NickDGreg 0:fc6fa085d591 18 AnalogIn analog_emg_left(A0);
NickDGreg 0:fc6fa085d591 19 PinName pinLeft = A0;
NickDGreg 0:fc6fa085d591 20 AnalogIn analog_emg_right(A1);
NickDGreg 0:fc6fa085d591 21 PinName pinRight = A1;
NickDGreg 0:fc6fa085d591 22
NickDGreg 0:fc6fa085d591 23 Ticker emg;
NickDGreg 0:fc6fa085d591 24
NickDGreg 0:fc6fa085d591 25 //encoder for the horizontal motor
NickDGreg 0:fc6fa085d591 26 QEI horizontal_encoder (PTC10, PTC11, NC, 624); // Pin for counting (analog in)
NickDGreg 0:fc6fa085d591 27
NickDGreg 0:fc6fa085d591 28 //encoder for the keypress motor
NickDGreg 0:fc6fa085d591 29 //QEI keypress_encoder (PTC10, PTC11, NC, 624); // Pin for counting (analog in)
NickDGreg 0:fc6fa085d591 30
NickDGreg 0:fc6fa085d591 31 //initialise the variables for the horizontal motor
NickDGreg 0:fc6fa085d591 32 DigitalOut dir_horizontal(D4);
NickDGreg 0:fc6fa085d591 33 PwmOut pwm_horizontal(D5);
NickDGreg 0:fc6fa085d591 34 double position_horizonal = horizontal_encoder.getPulses(); //actual position of horizontal movement motor
NickDGreg 0:fc6fa085d591 35 double setpoint_horizonal = 1; //desired position, needs to be in pulses?
NickDGreg 0:fc6fa085d591 36 double Kp_horizonal = 0; //controller gain
NickDGreg 0:fc6fa085d591 37 double Ki_horizontal = 0; //set to zero
NickDGreg 0:fc6fa085d591 38
NickDGreg 0:fc6fa085d591 39 //used to pass the value fo dir_horizontal by reference to the move motor funciton
NickDGreg 0:fc6fa085d591 40 double dir_hor_ref = dir_horizontal;
NickDGreg 0:fc6fa085d591 41 double pwm_hor_ref = pwm_horizontal;
NickDGreg 0:fc6fa085d591 42
NickDGreg 0:fc6fa085d591 43 //initialise the variables for the keypress motor
NickDGreg 0:fc6fa085d591 44 DigitalOut dir_keypress(D7); //direction
NickDGreg 0:fc6fa085d591 45 PwmOut pwm_keypress(D6); //speed
NickDGreg 0:fc6fa085d591 46
NickDGreg 0:fc6fa085d591 47 //change to keypress encoder!!
NickDGreg 0:fc6fa085d591 48 double position_keypress = horizontal_encoder.getPulses(); //actual position of keypress movement motor
NickDGreg 0:fc6fa085d591 49
NickDGreg 0:fc6fa085d591 50 double setpoint_keypress = 1; //desired position, always the same actually
NickDGreg 0:fc6fa085d591 51 double Kp_keypress = 0; //controller gain, different from left and right
NickDGreg 0:fc6fa085d591 52 double Ki_keypress = 0; //set to zero
NickDGreg 0:fc6fa085d591 53
NickDGreg 0:fc6fa085d591 54 double dir_keypress_ref = dir_keypress;
NickDGreg 0:fc6fa085d591 55 double pwm_keypress_ref = pwm_keypress;
NickDGreg 0:fc6fa085d591 56
NickDGreg 0:fc6fa085d591 57 //initialised to the value of on threashold (value needed to turn on the system)
NickDGreg 0:fc6fa085d591 58 //check_state_change alters the value depending on if the system has switched from on to off
NickDGreg 0:fc6fa085d591 59 double threashold = 10;
NickDGreg 0:fc6fa085d591 60
NickDGreg 0:fc6fa085d591 61 //tracks whether system is left,right,keypress,rest initialised to rest
NickDGreg 0:fc6fa085d591 62 std::string state = "rest";
NickDGreg 0:fc6fa085d591 63
NickDGreg 0:fc6fa085d591 64 //used to track how state from time to next time step
NickDGreg 0:fc6fa085d591 65 std::string newState;
NickDGreg 0:fc6fa085d591 66
NickDGreg 0:fc6fa085d591 67 //used to track the amount of time muscles are contracted, or newState changes over time
NickDGreg 0:fc6fa085d591 68 int num_on_inputs = 0;
NickDGreg 0:fc6fa085d591 69
NickDGreg 0:fc6fa085d591 70 //v1 and v2 are initialised to zero, they are updated in the emg.filter() function
NickDGreg 0:fc6fa085d591 71 double v1_left = 0;
NickDGreg 0:fc6fa085d591 72 double v2_left = 0;
NickDGreg 0:fc6fa085d591 73
NickDGreg 0:fc6fa085d591 74 double v1_right = 0;
NickDGreg 0:fc6fa085d591 75 double v2_right = 0;
NickDGreg 0:fc6fa085d591 76
NickDGreg 0:fc6fa085d591 77 int main() {
NickDGreg 0:fc6fa085d591 78 //create instance of left emg
NickDGreg 0:fc6fa085d591 79 //EMG emg_left(analog_emg_left, v1_left, v2_left);
NickDGreg 0:fc6fa085d591 80
NickDGreg 0:fc6fa085d591 81 EMG emg_left(pinLeft, v1_left, v2_left);
NickDGreg 0:fc6fa085d591 82
NickDGreg 0:fc6fa085d591 83 //create instance of right emg
NickDGreg 0:fc6fa085d591 84 //EMG emg_right(analog_emg_right, v1_right, v2_right);
NickDGreg 0:fc6fa085d591 85
NickDGreg 0:fc6fa085d591 86 EMG emg_right(pinRight, v1_right, v2_right);
NickDGreg 0:fc6fa085d591 87
NickDGreg 0:fc6fa085d591 88 /*THIS SECTION NEEDS TO BE CONTROLED USING TIMING*/
NickDGreg 0:fc6fa085d591 89 while(true)
NickDGreg 0:fc6fa085d591 90 {
NickDGreg 0:fc6fa085d591 91 //find value at current time
NickDGreg 0:fc6fa085d591 92 emg_left.sample();
NickDGreg 0:fc6fa085d591 93 //filter that value
NickDGreg 0:fc6fa085d591 94 double input_left = emg_left.filter(emg_left.input_sample);
NickDGreg 0:fc6fa085d591 95
NickDGreg 0:fc6fa085d591 96 //find value at current time
NickDGreg 0:fc6fa085d591 97 emg_right.sample();
NickDGreg 0:fc6fa085d591 98 //filter that value
NickDGreg 0:fc6fa085d591 99 double input_right = emg_right.filter(emg_right.input_sample);
NickDGreg 0:fc6fa085d591 100
NickDGreg 0:fc6fa085d591 101 //find which muscles are contracted above thresholds and return left,right,keypress or rest
NickDGreg 0:fc6fa085d591 102 newState = check_state(input_left, input_right, threashold);
NickDGreg 0:fc6fa085d591 103
NickDGreg 0:fc6fa085d591 104 //check whether new state = previous state and update num_on_inputs accordingly
NickDGreg 0:fc6fa085d591 105 check_state_change(state, newState, num_on_inputs, threashold);
NickDGreg 0:fc6fa085d591 106
NickDGreg 0:fc6fa085d591 107 //if num_on_inputs indicates same state for 0.25s move the necc. motor, if at rest just resets num_on_inputs
NickDGreg 0:fc6fa085d591 108 //update(state, num_on_inputs);
NickDGreg 0:fc6fa085d591 109 if (num_on_inputs == 250)
NickDGreg 0:fc6fa085d591 110 {
NickDGreg 0:fc6fa085d591 111 num_on_inputs = 0;
NickDGreg 0:fc6fa085d591 112 if (state == "left")
NickDGreg 0:fc6fa085d591 113 {
NickDGreg 0:fc6fa085d591 114 move_motor(dir_horizontal, pwm_horizontal, position_horizonal, setpoint_horizonal, Kp_horizonal, Ki_horizontal);
NickDGreg 0:fc6fa085d591 115 }
NickDGreg 0:fc6fa085d591 116 else if (state == "right")
NickDGreg 0:fc6fa085d591 117 {
NickDGreg 0:fc6fa085d591 118 //same as left but setpoint is negative so it moves in the other direction
NickDGreg 0:fc6fa085d591 119 move_motor(dir_horizontal, pwm_horizontal, position_horizonal, -setpoint_horizonal, Kp_horizonal, Ki_horizontal);
NickDGreg 0:fc6fa085d591 120 }
NickDGreg 0:fc6fa085d591 121 else if (state == "keypress")
NickDGreg 0:fc6fa085d591 122 {
NickDGreg 0:fc6fa085d591 123 move_motor(dir_keypress, pwm_keypress, position_keypress, setpoint_keypress, Kp_keypress, Ki_keypress);
NickDGreg 0:fc6fa085d591 124 //needs to move back to starting position so recall with opposite setpoint
NickDGreg 0:fc6fa085d591 125 move_motor(dir_keypress, pwm_keypress, position_keypress, setpoint_keypress, Kp_keypress, Ki_keypress);
NickDGreg 0:fc6fa085d591 126 }
NickDGreg 0:fc6fa085d591 127 //else rest do nothing
NickDGreg 0:fc6fa085d591 128 }
NickDGreg 0:fc6fa085d591 129 }
NickDGreg 0:fc6fa085d591 130 return 0;
NickDGreg 0:fc6fa085d591 131 }
NickDGreg 0:fc6fa085d591 132