Biorobotics / Mbed 2 deprecated piano_robot

newest version,

Dependencies:   QEI mbed

main.cpp@3:11c2175b4478, 2015-10-26 (annotated)

Committer:
roosbulthuis
Date:
Mon Oct 26 11:42:17 2015 +0000
Revision:
3:11c2175b4478
Parent:
1:5c1fc4e9401a 
New file for filters without classes, check if input and output are correct. Should be pasted into main code.

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 1:5c1fc4e9401a 79 EMG emg_left(analog_emg_left, v1_left, v2_left);
NickDGreg 0:fc6fa085d591 80
NickDGreg 1:5c1fc4e9401a 81 //EMG emg_left(pinLeft, v1_left, v2_left);
NickDGreg 0:fc6fa085d591 82
NickDGreg 0:fc6fa085d591 83 //create instance of right emg
NickDGreg 1:5c1fc4e9401a 84 EMG emg_right(analog_emg_right, v1_right, v2_right);
NickDGreg 0:fc6fa085d591 85
NickDGreg 1:5c1fc4e9401a 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