Controls the rovot with pot meters and button, fall back for if the filters fail
Dependencies: mbed
main.cpp@3:1f77b5e4eb42, 2015-10-29 (annotated)
- Committer:
- yohoo15
- Date:
- Thu Oct 29 14:39:37 2015 +0000
- Revision:
- 3:1f77b5e4eb42
- Parent:
- 2:90a9ed488f08
with begin of vader jacob
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
NickDGreg | 0:7b9ca8e5811b | 1 | #include "mbed.h" |
NickDGreg | 0:7b9ca8e5811b | 2 | |
NickDGreg | 0:7b9ca8e5811b | 3 | Serial pc(USBTX, USBRX); |
NickDGreg | 0:7b9ca8e5811b | 4 | |
NickDGreg | 0:7b9ca8e5811b | 5 | |
NickDGreg | 1:aa0d93df1af1 | 6 | // Define pins for motor control |
NickDGreg | 1:aa0d93df1af1 | 7 | //horizontal motor |
NickDGreg | 0:7b9ca8e5811b | 8 | DigitalOut directionPin(D4); |
NickDGreg | 0:7b9ca8e5811b | 9 | PwmOut PWM(D5); |
NickDGreg | 1:aa0d93df1af1 | 10 | //keypress motor |
NickDGreg | 0:7b9ca8e5811b | 11 | DigitalOut directionPin_key(D7); |
NickDGreg | 0:7b9ca8e5811b | 12 | PwmOut PWM_key(D6); |
NickDGreg | 0:7b9ca8e5811b | 13 | |
NickDGreg | 1:aa0d93df1af1 | 14 | //button to press the key |
NickDGreg | 1:aa0d93df1af1 | 15 | DigitalIn button_key(PTC6); |
yohoo15 | 3:1f77b5e4eb42 | 16 | DigitalIn button_song(PTA4); |
NickDGreg | 0:7b9ca8e5811b | 17 | |
NickDGreg | 1:aa0d93df1af1 | 18 | //pot meters to act as emg filter substitutes |
NickDGreg | 0:7b9ca8e5811b | 19 | AnalogIn left_pot(A0); |
NickDGreg | 0:7b9ca8e5811b | 20 | AnalogIn right_pot(A1); |
NickDGreg | 1:aa0d93df1af1 | 21 | |
NickDGreg | 1:aa0d93df1af1 | 22 | //variables to hold the values of the pot meters |
NickDGreg | 0:7b9ca8e5811b | 23 | double emg_left; |
NickDGreg | 0:7b9ca8e5811b | 24 | double emg_right; |
NickDGreg | 0:7b9ca8e5811b | 25 | |
NickDGreg | 1:aa0d93df1af1 | 26 | //int to check if the button is pressed |
NickDGreg | 0:7b9ca8e5811b | 27 | const int Button_move_pressed = 0; |
yohoo15 | 3:1f77b5e4eb42 | 28 | const int Button_song_pressed = 0; |
NickDGreg | 0:7b9ca8e5811b | 29 | |
NickDGreg | 1:aa0d93df1af1 | 30 | //values for clockwise and counter clockwise movement |
NickDGreg | 0:7b9ca8e5811b | 31 | const int cw = 1; |
NickDGreg | 0:7b9ca8e5811b | 32 | const int ccw = 0; |
NickDGreg | 0:7b9ca8e5811b | 33 | |
NickDGreg | 1:aa0d93df1af1 | 34 | |
NickDGreg | 0:7b9ca8e5811b | 35 | |
NickDGreg | 0:7b9ca8e5811b | 36 | int main() |
NickDGreg | 0:7b9ca8e5811b | 37 | { |
NickDGreg | 0:7b9ca8e5811b | 38 | while(1) { |
NickDGreg | 2:90a9ed488f08 | 39 | pc.printf("at the begin"); |
NickDGreg | 0:7b9ca8e5811b | 40 | |
NickDGreg | 1:aa0d93df1af1 | 41 | //read the 'emg' values |
NickDGreg | 0:7b9ca8e5811b | 42 | emg_left = left_pot.read(); |
NickDGreg | 0:7b9ca8e5811b | 43 | emg_right = right_pot.read(); |
NickDGreg | 0:7b9ca8e5811b | 44 | |
NickDGreg | 1:aa0d93df1af1 | 45 | //if left above threashold and right below; move left |
NickDGreg | 0:7b9ca8e5811b | 46 | while (emg_left > 0.5 and emg_right < 0.5) |
NickDGreg | 0:7b9ca8e5811b | 47 | { |
NickDGreg | 0:7b9ca8e5811b | 48 | directionPin.write(cw); |
NickDGreg | 0:7b9ca8e5811b | 49 | PWM.write(1); |
NickDGreg | 1:aa0d93df1af1 | 50 | //new left value to check (in while loop condition) if it has fallen below threashold |
NickDGreg | 0:7b9ca8e5811b | 51 | emg_left = left_pot.read(); |
NickDGreg | 2:90a9ed488f08 | 52 | pc.printf("left \n"); |
NickDGreg | 0:7b9ca8e5811b | 53 | } |
NickDGreg | 0:7b9ca8e5811b | 54 | |
NickDGreg | 0:7b9ca8e5811b | 55 | while (emg_right > 0.5 and emg_left < 0.5) |
NickDGreg | 0:7b9ca8e5811b | 56 | { |
NickDGreg | 0:7b9ca8e5811b | 57 | directionPin.write(ccw); |
NickDGreg | 0:7b9ca8e5811b | 58 | PWM.write(1); |
NickDGreg | 1:aa0d93df1af1 | 59 | //new right value, to check in while loop condition |
NickDGreg | 0:7b9ca8e5811b | 60 | emg_right = right_pot.read(); |
NickDGreg | 2:90a9ed488f08 | 61 | pc.printf("right \n"); |
NickDGreg | 0:7b9ca8e5811b | 62 | } |
NickDGreg | 1:aa0d93df1af1 | 63 | if (button_key.read() == Button_move_pressed) |
NickDGreg | 0:7b9ca8e5811b | 64 | { |
NickDGreg | 0:7b9ca8e5811b | 65 | //move to key |
NickDGreg | 0:7b9ca8e5811b | 66 | directionPin_key.write(cw); |
NickDGreg | 1:aa0d93df1af1 | 67 | PWM_key.write(1); |
NickDGreg | 1:aa0d93df1af1 | 68 | wait(0.14); //time it takes pwm of one to reach the key |
NickDGreg | 1:aa0d93df1af1 | 69 | //pause at the key for one second |
NickDGreg | 1:aa0d93df1af1 | 70 | PWM_key.write(0); |
NickDGreg | 1:aa0d93df1af1 | 71 | wait(1); |
NickDGreg | 1:aa0d93df1af1 | 72 | //move bakc to the starting position |
NickDGreg | 0:7b9ca8e5811b | 73 | directionPin_key.write(ccw); |
NickDGreg | 1:aa0d93df1af1 | 74 | PWM_key.write(1); |
NickDGreg | 1:aa0d93df1af1 | 75 | //time needs to be slightly longer than moving forward (no answer why) |
NickDGreg | 1:aa0d93df1af1 | 76 | wait(0.165f); |
NickDGreg | 2:90a9ed488f08 | 77 | pc.printf("keypress \n"); |
NickDGreg | 0:7b9ca8e5811b | 78 | |
NickDGreg | 0:7b9ca8e5811b | 79 | } |
NickDGreg | 1:aa0d93df1af1 | 80 | |
NickDGreg | 1:aa0d93df1af1 | 81 | /// set spped values of both motors to zero so if no muscles are contracted, no motors move |
NickDGreg | 1:aa0d93df1af1 | 82 | //more of a safety catch than actually necessary |
NickDGreg | 0:7b9ca8e5811b | 83 | PWM_key.write(0); |
NickDGreg | 0:7b9ca8e5811b | 84 | PWM.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 85 | |
yohoo15 | 3:1f77b5e4eb42 | 86 | //cw to the right |
yohoo15 | 3:1f77b5e4eb42 | 87 | if (button_song.read() == Button_song_pressed){ |
yohoo15 | 3:1f77b5e4eb42 | 88 | |
yohoo15 | 3:1f77b5e4eb42 | 89 | //pressing va |
yohoo15 | 3:1f77b5e4eb42 | 90 | directionPin_key.write(cw); |
yohoo15 | 3:1f77b5e4eb42 | 91 | PWM_key.write(1); |
yohoo15 | 3:1f77b5e4eb42 | 92 | wait(0.14); //time it takes pwm of one to reach the key |
yohoo15 | 3:1f77b5e4eb42 | 93 | //pause at the key for one second |
yohoo15 | 3:1f77b5e4eb42 | 94 | PWM_key.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 95 | wait(1); |
yohoo15 | 3:1f77b5e4eb42 | 96 | //move bakc to the starting position |
yohoo15 | 3:1f77b5e4eb42 | 97 | directionPin_key.write(ccw); |
yohoo15 | 3:1f77b5e4eb42 | 98 | PWM_key.write(1); |
yohoo15 | 3:1f77b5e4eb42 | 99 | //time needs to be slightly longer than moving forward (no answer why) |
yohoo15 | 3:1f77b5e4eb42 | 100 | wait(0.165f); |
yohoo15 | 3:1f77b5e4eb42 | 101 | PWM_key.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 102 | |
yohoo15 | 3:1f77b5e4eb42 | 103 | // go 1 key to right |
yohoo15 | 3:1f77b5e4eb42 | 104 | directionPin.write(cw); |
yohoo15 | 3:1f77b5e4eb42 | 105 | PWM.write(1); |
yohoo15 | 3:1f77b5e4eb42 | 106 | wait(2.2); |
yohoo15 | 3:1f77b5e4eb42 | 107 | PWM.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 108 | |
yohoo15 | 3:1f77b5e4eb42 | 109 | //pressing der |
yohoo15 | 3:1f77b5e4eb42 | 110 | directionPin_key.write(cw); |
yohoo15 | 3:1f77b5e4eb42 | 111 | PWM_key.write(1); |
yohoo15 | 3:1f77b5e4eb42 | 112 | wait(0.14); //time it takes pwm of one to reach the key |
yohoo15 | 3:1f77b5e4eb42 | 113 | //pause at the key for one second |
yohoo15 | 3:1f77b5e4eb42 | 114 | PWM_key.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 115 | wait(1); |
yohoo15 | 3:1f77b5e4eb42 | 116 | //move bakc to the starting position |
yohoo15 | 3:1f77b5e4eb42 | 117 | directionPin_key.write(ccw); |
yohoo15 | 3:1f77b5e4eb42 | 118 | PWM_key.write(1); |
yohoo15 | 3:1f77b5e4eb42 | 119 | //time needs to be slightly longer than moving forward (no answer why) |
yohoo15 | 3:1f77b5e4eb42 | 120 | wait(0.165f); |
yohoo15 | 3:1f77b5e4eb42 | 121 | PWM_key.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 122 | |
yohoo15 | 3:1f77b5e4eb42 | 123 | // go 1 key to right |
yohoo15 | 3:1f77b5e4eb42 | 124 | directionPin.write(cw); |
yohoo15 | 3:1f77b5e4eb42 | 125 | PWM.write(1); |
yohoo15 | 3:1f77b5e4eb42 | 126 | wait(2.2); |
yohoo15 | 3:1f77b5e4eb42 | 127 | PWM.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 128 | |
yohoo15 | 3:1f77b5e4eb42 | 129 | //pressing ja |
yohoo15 | 3:1f77b5e4eb42 | 130 | directionPin_key.write(cw); |
yohoo15 | 3:1f77b5e4eb42 | 131 | PWM_key.write(1); |
yohoo15 | 3:1f77b5e4eb42 | 132 | wait(0.14); //time it takes pwm of one to reach the key |
yohoo15 | 3:1f77b5e4eb42 | 133 | //pause at the key for one second |
yohoo15 | 3:1f77b5e4eb42 | 134 | PWM_key.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 135 | wait(1); |
yohoo15 | 3:1f77b5e4eb42 | 136 | //move bakc to the starting position |
yohoo15 | 3:1f77b5e4eb42 | 137 | directionPin_key.write(ccw); |
yohoo15 | 3:1f77b5e4eb42 | 138 | PWM_key.write(1); |
yohoo15 | 3:1f77b5e4eb42 | 139 | //time needs to be slightly longer than moving forward (no answer why) |
yohoo15 | 3:1f77b5e4eb42 | 140 | wait(0.165f); |
yohoo15 | 3:1f77b5e4eb42 | 141 | PWM_key.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 142 | |
yohoo15 | 3:1f77b5e4eb42 | 143 | // go 2 key to left |
yohoo15 | 3:1f77b5e4eb42 | 144 | directionPin.write(ccw); |
yohoo15 | 3:1f77b5e4eb42 | 145 | PWM.write(1); |
yohoo15 | 3:1f77b5e4eb42 | 146 | wait(4.5); |
yohoo15 | 3:1f77b5e4eb42 | 147 | PWM.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 148 | |
yohoo15 | 3:1f77b5e4eb42 | 149 | //pressing cob |
yohoo15 | 3:1f77b5e4eb42 | 150 | directionPin_key.write(cw); |
yohoo15 | 3:1f77b5e4eb42 | 151 | PWM_key.write(1); |
yohoo15 | 3:1f77b5e4eb42 | 152 | wait(0.14); //time it takes pwm of one to reach the key |
yohoo15 | 3:1f77b5e4eb42 | 153 | //pause at the key for one second |
yohoo15 | 3:1f77b5e4eb42 | 154 | PWM_key.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 155 | wait(1); |
yohoo15 | 3:1f77b5e4eb42 | 156 | //move bakc to the starting position |
yohoo15 | 3:1f77b5e4eb42 | 157 | directionPin_key.write(ccw); |
yohoo15 | 3:1f77b5e4eb42 | 158 | PWM_key.write(1); |
yohoo15 | 3:1f77b5e4eb42 | 159 | //time needs to be slightly longer than moving forward (no answer why) |
yohoo15 | 3:1f77b5e4eb42 | 160 | wait(0.165f); |
yohoo15 | 3:1f77b5e4eb42 | 161 | PWM_key.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 162 | |
yohoo15 | 3:1f77b5e4eb42 | 163 | wait(1.5); |
yohoo15 | 3:1f77b5e4eb42 | 164 | |
yohoo15 | 3:1f77b5e4eb42 | 165 | //pressing va |
yohoo15 | 3:1f77b5e4eb42 | 166 | directionPin_key.write(cw); |
yohoo15 | 3:1f77b5e4eb42 | 167 | PWM_key.write(1); |
yohoo15 | 3:1f77b5e4eb42 | 168 | wait(0.14); //time it takes pwm of one to reach the key |
yohoo15 | 3:1f77b5e4eb42 | 169 | //pause at the key for one second |
yohoo15 | 3:1f77b5e4eb42 | 170 | PWM_key.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 171 | wait(1); |
yohoo15 | 3:1f77b5e4eb42 | 172 | //move bakc to the starting position |
yohoo15 | 3:1f77b5e4eb42 | 173 | directionPin_key.write(ccw); |
yohoo15 | 3:1f77b5e4eb42 | 174 | PWM_key.write(1); |
yohoo15 | 3:1f77b5e4eb42 | 175 | //time needs to be slightly longer than moving forward (no answer why) |
yohoo15 | 3:1f77b5e4eb42 | 176 | wait(0.165f); |
yohoo15 | 3:1f77b5e4eb42 | 177 | PWM_key.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 178 | |
yohoo15 | 3:1f77b5e4eb42 | 179 | // go 1 key to right |
yohoo15 | 3:1f77b5e4eb42 | 180 | directionPin.write(cw); |
yohoo15 | 3:1f77b5e4eb42 | 181 | PWM.write(1); |
yohoo15 | 3:1f77b5e4eb42 | 182 | wait(2.2); |
yohoo15 | 3:1f77b5e4eb42 | 183 | PWM.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 184 | |
yohoo15 | 3:1f77b5e4eb42 | 185 | //pressing der |
yohoo15 | 3:1f77b5e4eb42 | 186 | directionPin_key.write(cw); |
yohoo15 | 3:1f77b5e4eb42 | 187 | PWM_key.write(1); |
yohoo15 | 3:1f77b5e4eb42 | 188 | wait(0.14); //time it takes pwm of one to reach the key |
yohoo15 | 3:1f77b5e4eb42 | 189 | //pause at the key for one second |
yohoo15 | 3:1f77b5e4eb42 | 190 | PWM_key.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 191 | wait(1); |
yohoo15 | 3:1f77b5e4eb42 | 192 | //move bakc to the starting position |
yohoo15 | 3:1f77b5e4eb42 | 193 | directionPin_key.write(ccw); |
yohoo15 | 3:1f77b5e4eb42 | 194 | PWM_key.write(1); |
yohoo15 | 3:1f77b5e4eb42 | 195 | //time needs to be slightly longer than moving forward (no answer why) |
yohoo15 | 3:1f77b5e4eb42 | 196 | wait(0.165f); |
yohoo15 | 3:1f77b5e4eb42 | 197 | PWM_key.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 198 | |
yohoo15 | 3:1f77b5e4eb42 | 199 | // go 1 key to right |
yohoo15 | 3:1f77b5e4eb42 | 200 | directionPin.write(cw); |
yohoo15 | 3:1f77b5e4eb42 | 201 | PWM.write(1); |
yohoo15 | 3:1f77b5e4eb42 | 202 | wait(2.2); |
yohoo15 | 3:1f77b5e4eb42 | 203 | PWM.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 204 | |
yohoo15 | 3:1f77b5e4eb42 | 205 | //pressing ja |
yohoo15 | 3:1f77b5e4eb42 | 206 | directionPin_key.write(cw); |
yohoo15 | 3:1f77b5e4eb42 | 207 | PWM_key.write(1); |
yohoo15 | 3:1f77b5e4eb42 | 208 | wait(0.14); //time it takes pwm of one to reach the key |
yohoo15 | 3:1f77b5e4eb42 | 209 | //pause at the key for one second |
yohoo15 | 3:1f77b5e4eb42 | 210 | PWM_key.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 211 | wait(1); |
yohoo15 | 3:1f77b5e4eb42 | 212 | //move bakc to the starting position |
yohoo15 | 3:1f77b5e4eb42 | 213 | directionPin_key.write(ccw); |
yohoo15 | 3:1f77b5e4eb42 | 214 | PWM_key.write(1); |
yohoo15 | 3:1f77b5e4eb42 | 215 | //time needs to be slightly longer than moving forward (no answer why) |
yohoo15 | 3:1f77b5e4eb42 | 216 | wait(0.165f); |
yohoo15 | 3:1f77b5e4eb42 | 217 | PWM_key.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 218 | |
yohoo15 | 3:1f77b5e4eb42 | 219 | // go 2 key to left |
yohoo15 | 3:1f77b5e4eb42 | 220 | directionPin.write(ccw); |
yohoo15 | 3:1f77b5e4eb42 | 221 | PWM.write(1); |
yohoo15 | 3:1f77b5e4eb42 | 222 | wait(4.5); |
yohoo15 | 3:1f77b5e4eb42 | 223 | PWM.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 224 | |
yohoo15 | 3:1f77b5e4eb42 | 225 | //pressing cob |
yohoo15 | 3:1f77b5e4eb42 | 226 | directionPin_key.write(cw); |
yohoo15 | 3:1f77b5e4eb42 | 227 | PWM_key.write(1); |
yohoo15 | 3:1f77b5e4eb42 | 228 | wait(0.14); //time it takes pwm of one to reach the key |
yohoo15 | 3:1f77b5e4eb42 | 229 | //pause at the key for one second |
yohoo15 | 3:1f77b5e4eb42 | 230 | PWM_key.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 231 | wait(1); |
yohoo15 | 3:1f77b5e4eb42 | 232 | //move bakc to the starting position |
yohoo15 | 3:1f77b5e4eb42 | 233 | directionPin_key.write(ccw); |
yohoo15 | 3:1f77b5e4eb42 | 234 | PWM_key.write(1); |
yohoo15 | 3:1f77b5e4eb42 | 235 | //time needs to be slightly longer than moving forward (no answer why) |
yohoo15 | 3:1f77b5e4eb42 | 236 | wait(0.165f); |
yohoo15 | 3:1f77b5e4eb42 | 237 | PWM_key.write(0); |
yohoo15 | 3:1f77b5e4eb42 | 238 | } |
NickDGreg | 0:7b9ca8e5811b | 239 | } |
NickDGreg | 0:7b9ca8e5811b | 240 | } |
NickDGreg | 0:7b9ca8e5811b | 241 |