Team Ascent / Mbed 2 deprecated TDPCode1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@7:38def1917030, 2015-03-11 (annotated)

Committer:
Dbee16
Date:
Wed Mar 11 23:42:10 2015 +0000
Revision:
7:38def1917030
Parent:
4:6db8e0babea7
Child:
8:eefbd3880d28
Ready for bluetooth test, i think

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cmcmaster 0:59d25bb7f825 1 #include "mbed.h"
cmcmaster 0:59d25bb7f825 2
cmcmaster 0:59d25bb7f825 3 //motor select pins
cmcmaster 0:59d25bb7f825 4 DigitalOut motor_lf(PTE2);
cmcmaster 0:59d25bb7f825 5 DigitalOut motor_lb(PTE3);
cmcmaster 0:59d25bb7f825 6 DigitalOut motor_rf(PTE4);
cmcmaster 0:59d25bb7f825 7 DigitalOut motor_rb(PTE5);
cmcmaster 0:59d25bb7f825 8
cmcmaster 0:59d25bb7f825 9 //Frequency of Pulse Width Modulated signal in Hz
cmcmaster 0:59d25bb7f825 10 #define PWM_FREQ 1000
cmcmaster 0:59d25bb7f825 11
cmcmaster 0:59d25bb7f825 12 //PWM pin (Enable 1 and 2)
cmcmaster 0:59d25bb7f825 13 PwmOut motor_l (PTC2);
cmcmaster 0:59d25bb7f825 14 PwmOut motor_r (PTE29);
cmcmaster 0:59d25bb7f825 15
cmcmaster 0:59d25bb7f825 16 //LED to test
Dbee16 4:6db8e0babea7 17 DigitalOut led(LED_RED);
Dbee16 1:2bab3a0bc3bc 18
Dbee16 7:38def1917030 19 //Bluetooth Module to debug
Dbee16 7:38def1917030 20 Serial blue(PTC4,PTC3);
Dbee16 7:38def1917030 21
Dbee16 2:1feae3cb6731 22 void set_direction( int direction, float speed, float angle)
cmcmaster 0:59d25bb7f825 23 {
Dbee16 1:2bab3a0bc3bc 24 /* *******************Example Use*****************
Dbee16 2:1feae3cb6731 25 set_direction("11", 0.5, 0); //This would set the robot to go forward, at half speed, straight
Dbee16 2:1feae3cb6731 26
Dbee16 2:1feae3cb6731 27 set_directin( "11", 0.8, 0.2); //this would set robot to go forward, at 100% to the right and 60% to the left (turn right)
Dbee16 1:2bab3a0bc3bc 28
Dbee16 2:1feae3cb6731 29 set_direction("00", 0.3, 0.7); //robot back, 100% rightmotor; 0%left (technically -40%, but 0% is min)
Dbee16 1:2bab3a0bc3bc 30
Dbee16 2:1feae3cb6731 31 set_direction("11", 0.5, 0.5); // robot pivots forward, to the right, full speed (100% duty right ; 0% duty left)
Dbee16 1:2bab3a0bc3bc 32
Dbee16 2:1feae3cb6731 33 set_direction("11", 0.5, -0.5); //robot pivots forward, to the left, full speed (0% duty right ; 100% duty left)
Dbee16 1:2bab3a0bc3bc 34
Dbee16 2:1feae3cb6731 35 set_direction("11", 0.3, 0.5); //robot pivots forard, to the right , at 80% speed (80% right; 0% left)
Dbee16 1:2bab3a0bc3bc 36
Dbee16 2:1feae3cb6731 37 set_direction("11", -0.3, 0.5); //robot pivotrs foward, to the right, AT 20% SPEED (20%, 0%)
Dbee16 1:2bab3a0bc3bc 38
Dbee16 2:1feae3cb6731 39 set_direction("10", 0.5, 0); //robot spins to teh right, at half speed. Left forward 50% ; Right forward 50%
Dbee16 2:1feae3cb6731 40
Dbee16 2:1feae3cb6731 41 set_direction("10", 0.5, 0.3); //robot spins to the right, 80% right forward ; 20% left backward
Dbee16 1:2bab3a0bc3bc 42
Dbee16 1:2bab3a0bc3bc 43 */
Dbee16 2:1feae3cb6731 44
Dbee16 2:1feae3cb6731 45 float r_result = (speed + angle); //where angle can be postive and negative and will TURN the robot
Dbee16 2:1feae3cb6731 46 float l_result = (speed - angle); //if turning left, you'd make the angle postive (right motor goes faster left motor goes slwoer)
Dbee16 2:1feae3cb6731 47 switch( direction ) {
Dbee16 2:1feae3cb6731 48 case 0x11: { //forward
Dbee16 2:1feae3cb6731 49 motor_r.write( r_result < 0 ? 0 : r_result > 1 ? 1 : r_result);
Dbee16 2:1feae3cb6731 50 //Look up ternary statements -- {condition ? value_if_true : value_if_false} This makes 0<result<1
Dbee16 2:1feae3cb6731 51 motor_l.write( l_result < 0 ? 0 : l_result > 1 ? 1 : l_result);
Dbee16 2:1feae3cb6731 52
cmcmaster 0:59d25bb7f825 53 motor_rf=1;
cmcmaster 0:59d25bb7f825 54 motor_rb=0;
cmcmaster 0:59d25bb7f825 55 motor_lf=1;
cmcmaster 0:59d25bb7f825 56 motor_lb=0;
Dbee16 7:38def1917030 57 blue.printf("I'm going forwards, r_motor duty is %f ; l_motor duty is %f", r_result, l_result);
cmcmaster 0:59d25bb7f825 58 }
Dbee16 2:1feae3cb6731 59 case 0x00: { //backward
Dbee16 2:1feae3cb6731 60 motor_r.write( r_result < 0 ? 0 : r_result > 1 ? 1 : r_result);
Dbee16 2:1feae3cb6731 61 motor_l.write( l_result < 0 ? 0 : l_result > 1 ? 1 : l_result);
Dbee16 2:1feae3cb6731 62
cmcmaster 0:59d25bb7f825 63 motor_rf=0;
cmcmaster 0:59d25bb7f825 64 motor_rb=1;
cmcmaster 0:59d25bb7f825 65 motor_lf=0;
cmcmaster 0:59d25bb7f825 66 motor_lb=1;
Dbee16 7:38def1917030 67 blue.printf("I'm going backwards, r_motor duty is %f ; l_motor duty is %f", r_result, l_result);
Dbee16 7:38def1917030 68
cmcmaster 0:59d25bb7f825 69 }
Dbee16 2:1feae3cb6731 70 case 0x01: { //spin left -- Right forward, left backward
Dbee16 2:1feae3cb6731 71 motor_r.write( r_result < 0 ? 0 : r_result > 1 ? 1 : r_result);
Dbee16 2:1feae3cb6731 72 motor_l.write( l_result < 0 ? 0 : l_result > 1 ? 1 : l_result);
Dbee16 2:1feae3cb6731 73
Dbee16 2:1feae3cb6731 74 motor_rf=1;
Dbee16 2:1feae3cb6731 75 motor_rb=0;
Dbee16 2:1feae3cb6731 76 motor_lf=0;
Dbee16 2:1feae3cb6731 77 motor_lb=1;
Dbee16 7:38def1917030 78 blue.printf("I'm spinning left, r_motor duty is %f ; l_motor duty is %f", r_result, l_result);
Dbee16 7:38def1917030 79
Dbee16 2:1feae3cb6731 80 }
Dbee16 2:1feae3cb6731 81 case 0x10: { //spin right
Dbee16 2:1feae3cb6731 82 motor_r.write( r_result < 0 ? 0 : r_result > 1 ? 1 : r_result);
Dbee16 2:1feae3cb6731 83 motor_l.write( l_result < 0 ? 0 : l_result > 1 ? 1 : l_result);
Dbee16 2:1feae3cb6731 84
Dbee16 2:1feae3cb6731 85 motor_rf=0;
Dbee16 2:1feae3cb6731 86 motor_rb=1;
Dbee16 2:1feae3cb6731 87 motor_lf=1;
Dbee16 2:1feae3cb6731 88 motor_lb=0;
Dbee16 7:38def1917030 89 blue.printf("I'm spinning right, r_motor duty is %f ; l_motor duty is %f", r_result, l_result);
Dbee16 7:38def1917030 90
Dbee16 2:1feae3cb6731 91 }
cmcmaster 0:59d25bb7f825 92 }
cmcmaster 0:59d25bb7f825 93 }
cmcmaster 0:59d25bb7f825 94 int main()
cmcmaster 0:59d25bb7f825 95 {
Dbee16 2:1feae3cb6731 96 //Set PWM frequency to 1000Hz
cmcmaster 0:59d25bb7f825 97 motor_l.period( 1.0f / (float) PWM_FREQ);
cmcmaster 0:59d25bb7f825 98 motor_r.period( 1.0f / (float) PWM_FREQ);
cmcmaster 0:59d25bb7f825 99 //Initialise direction to nothing.
cmcmaster 0:59d25bb7f825 100 motor_rf=0;
cmcmaster 0:59d25bb7f825 101 motor_rb=0;
cmcmaster 0:59d25bb7f825 102 motor_lf=0;
cmcmaster 0:59d25bb7f825 103 motor_lb=0;
Dbee16 7:38def1917030 104 int direction = 0x11;
Dbee16 7:38def1917030 105 char rem_dir[2]; //remote_direction
cmcmaster 0:59d25bb7f825 106
Dbee16 1:2bab3a0bc3bc 107 while(1) {
Dbee16 1:2bab3a0bc3bc 108
Dbee16 1:2bab3a0bc3bc 109
Dbee16 7:38def1917030 110 wait(1);
Dbee16 7:38def1917030 111 blue.printf("Gees a Direction\nFF for forward both, BB for backward both, FB for forward left and back right .. etc. \n");
Dbee16 7:38def1917030 112
Dbee16 7:38def1917030 113 while ( blue.readable()==0 ) {} // loops until there is an input from the user
Dbee16 7:38def1917030 114
Dbee16 7:38def1917030 115 for(int i=0; i<2; i++) {
Dbee16 7:38def1917030 116 rem_dir[i] = blue.getc(); //Store incoming response into array
Dbee16 7:38def1917030 117 }
Dbee16 1:2bab3a0bc3bc 118
Dbee16 7:38def1917030 119 //0x46 is F, 0x66 is f
Dbee16 7:38def1917030 120 //0x42 is B , 0x62 is b
Dbee16 7:38def1917030 121 if( (rem_dir[0] == 'F'|'f' ) && ( rem_dir[1] == 'F'|'f') ) {
Dbee16 7:38def1917030 122 direction = 0x11;
Dbee16 7:38def1917030 123 blue.printf("You wanted to go straight\n");
Dbee16 7:38def1917030 124 } else if ( (rem_dir[0] == 'B'|'b' ) && ( rem_dir[1] == 'B'|'b') ) {
Dbee16 7:38def1917030 125
Dbee16 7:38def1917030 126 direction = 0x00;
Dbee16 7:38def1917030 127 blue.printf("You wanted to go backwards\n");
Dbee16 7:38def1917030 128
Dbee16 7:38def1917030 129 } else if ( (rem_dir[0] == 'B'|'b' ) && ( rem_dir[1] == 'F'|'f') ) {
cmcmaster 0:59d25bb7f825 130
Dbee16 7:38def1917030 131 direction = 0x01;
Dbee16 7:38def1917030 132 blue.printf("You wanted to spin anti-clockwise\n");
Dbee16 7:38def1917030 133
Dbee16 7:38def1917030 134 } else if ( (rem_dir[0] == 'F'|'f' ) && ( rem_dir[1] == 'B'|'b') ) {
Dbee16 7:38def1917030 135
Dbee16 7:38def1917030 136 direction = 0x10;
Dbee16 7:38def1917030 137 blue.printf("You wanted to spin clockwise\n");
Dbee16 7:38def1917030 138 } else {}
Dbee16 7:38def1917030 139 set_direction(direction, 0.5, 0);
cmcmaster 0:59d25bb7f825 140
Dbee16 7:38def1917030 141 /* SETTING SPEED AND ANGLE TOO
Dbee16 7:38def1917030 142 blue.printf("Whit speed you going at?\n Send it as a duty percentage");
Dbee16 7:38def1917030 143 while ( blue.readable()==0 ) {} // loops until there is an input from the user
Dbee16 7:38def1917030 144
Dbee16 7:38def1917030 145 speed = input;
Dbee16 7:38def1917030 146
Dbee16 7:38def1917030 147 blue.printf("What angle you going at? percentage again (negative percentage for opposite dir)");
Dbee16 7:38def1917030 148 while ( blue.readable()==0 ) {} // loops until there is an input from the user
Dbee16 7:38def1917030 149
Dbee16 7:38def1917030 150 angle = input;
Dbee16 7:38def1917030 151 */
cmcmaster 0:59d25bb7f825 152
Dbee16 7:38def1917030 153 //set_direction(direction, speed, angle);
Dbee16 7:38def1917030 154
Dbee16 7:38def1917030 155 }
Dbee16 7:38def1917030 156
Dbee16 7:38def1917030 157 /* //Also Working Demo
Dbee16 7:38def1917030 158 wait(2);
Dbee16 7:38def1917030 159 led = !led;
Dbee16 7:38def1917030 160 set_direction(0x11, 0.5, 0); //This would set the robot to go forward, at half speed, straight
cmcmaster 0:59d25bb7f825 161
Dbee16 7:38def1917030 162 wait(2);
Dbee16 7:38def1917030 163 led = !led;
Dbee16 7:38def1917030 164 set_direction(0x11, 0.8, 0.2); //this would set robot to go forward, at 100% to the right and 60% to the left (turn right)
Dbee16 7:38def1917030 165
Dbee16 7:38def1917030 166 wait(2);
Dbee16 7:38def1917030 167 led = !led;
Dbee16 7:38def1917030 168 set_direction(0x00, 0.3, 0.7); //robot back, 100% rightmotor; 0%left (technically -40%, but 0% is min)
Dbee16 7:38def1917030 169
Dbee16 7:38def1917030 170 wait(2);
Dbee16 7:38def1917030 171 led = !led;
Dbee16 7:38def1917030 172 set_direction(0x11, 0.5, 0.5); // robot pivots forward, to the right, full speed (100% duty right ; 0% duty left)
cmcmaster 0:59d25bb7f825 173
Dbee16 7:38def1917030 174 wait(2);
Dbee16 7:38def1917030 175 led = !led;
Dbee16 7:38def1917030 176 set_direction(0x11, 0.5, -0.5); //robot pivots forward, to the left, full speed (0% duty right ; 100% duty left)
cmcmaster 0:59d25bb7f825 177
Dbee16 7:38def1917030 178 wait(2);
Dbee16 7:38def1917030 179 led = !led;
Dbee16 7:38def1917030 180 set_direction(0x11, 0.3, 0.5); //robot pivots forard, to the right , at 80% speed (80% right; 0% left)
cmcmaster 0:59d25bb7f825 181
Dbee16 7:38def1917030 182 wait(2);
Dbee16 7:38def1917030 183 led = !led;
Dbee16 7:38def1917030 184 set_direction(0x11, -0.3, 0.5); //robot pivotrs foward, to the right, AT 20% SPEED (20%, 0%)
Dbee16 7:38def1917030 185
Dbee16 7:38def1917030 186 wait(2);
Dbee16 7:38def1917030 187 led = !led;
Dbee16 7:38def1917030 188 set_direction(0x10, 0.5, 0); //robot spins to teh right, at half speed. Left forward 50% ; Right forward 50%
Dbee16 7:38def1917030 189
Dbee16 7:38def1917030 190 wait(2);
Dbee16 7:38def1917030 191 led = !led;
Dbee16 7:38def1917030 192 set_direction(0x10, 0.5, 0.3); //robot spins to the right, 80% right forward ; 20% left backward
Dbee16 7:38def1917030 193
Dbee16 7:38def1917030 194 }*/
cmcmaster 0:59d25bb7f825 195
cmcmaster 0:59d25bb7f825 196
Dbee16 7:38def1917030 197 /* // working demo
Dbee16 7:38def1917030 198
Dbee16 7:38def1917030 199 while(1) { //infinite loop
Dbee16 7:38def1917030 200 //***********************************************
Dbee16 7:38def1917030 201 myled = !myled;
Dbee16 7:38def1917030 202
Dbee16 7:38def1917030 203 //Set duty cycle at 100% AKA Full Speed
Dbee16 7:38def1917030 204 motor_r.write(1.0);
Dbee16 7:38def1917030 205 motor_l.write(1.0);
cmcmaster 0:59d25bb7f825 206
Dbee16 7:38def1917030 207 motor_rf=1;
Dbee16 7:38def1917030 208 motor_rb=0;
Dbee16 7:38def1917030 209 motor_lf=1;
Dbee16 7:38def1917030 210 motor_lb=0; //go forwards
Dbee16 7:38def1917030 211 wait(2);
Dbee16 7:38def1917030 212 //**************************************
Dbee16 7:38def1917030 213 myled = !myled;
cmcmaster 0:59d25bb7f825 214
Dbee16 7:38def1917030 215 motor_r.write(0.75);
Dbee16 7:38def1917030 216 motor_l.write(0.75);
Dbee16 7:38def1917030 217
Dbee16 7:38def1917030 218 motor_rf=1;
Dbee16 7:38def1917030 219 motor_rb=0;
Dbee16 7:38def1917030 220 motor_lf=1;
Dbee16 7:38def1917030 221 motor_lb=0; //go forwards
Dbee16 7:38def1917030 222 //
Dbee16 7:38def1917030 223 wait(2);
Dbee16 7:38def1917030 224
Dbee16 7:38def1917030 225 myled = !myled;
cmcmaster 0:59d25bb7f825 226
Dbee16 7:38def1917030 227 motor_r.write(0.5);
Dbee16 7:38def1917030 228 motor_l.write(0.5);
cmcmaster 0:59d25bb7f825 229
Dbee16 7:38def1917030 230 motor_rf=1;
Dbee16 7:38def1917030 231 motor_rb=0;
Dbee16 7:38def1917030 232 motor_lf=1;
Dbee16 7:38def1917030 233 motor_lb=0;
Dbee16 7:38def1917030 234
Dbee16 7:38def1917030 235
Dbee16 7:38def1917030 236 wait(2);
Dbee16 7:38def1917030 237 myled = !myled;
Dbee16 7:38def1917030 238
Dbee16 7:38def1917030 239 motor_r.write(0.30);
Dbee16 7:38def1917030 240 motor_l.write(0.30);
cmcmaster 0:59d25bb7f825 241
Dbee16 7:38def1917030 242 motor_rf=1;
Dbee16 7:38def1917030 243 motor_rb=0;
Dbee16 7:38def1917030 244 motor_lf=1;
Dbee16 7:38def1917030 245 motor_lb=0;
Dbee16 7:38def1917030 246
Dbee16 7:38def1917030 247 wait(2);
Dbee16 7:38def1917030 248
Dbee16 7:38def1917030 249 myled = !myled;
Dbee16 7:38def1917030 250 motor_r.write(1.0);
Dbee16 7:38def1917030 251 motor_l.write(1.0);
Dbee16 7:38def1917030 252
Dbee16 7:38def1917030 253 motor_rf=0;
Dbee16 7:38def1917030 254 motor_rb=1;
Dbee16 7:38def1917030 255 motor_lf=0;
Dbee16 7:38def1917030 256 motor_lb=1; //go backwards
Dbee16 7:38def1917030 257 //
Dbee16 7:38def1917030 258 wait(2);
cmcmaster 0:59d25bb7f825 259
cmcmaster 0:59d25bb7f825 260
cmcmaster 0:59d25bb7f825 261
Dbee16 7:38def1917030 262 myled = !myled;
Dbee16 7:38def1917030 263 motor_r.write(0.5);
Dbee16 7:38def1917030 264 motor_l.write(0.5);
cmcmaster 0:59d25bb7f825 265
Dbee16 7:38def1917030 266 motor_rf=0;
Dbee16 7:38def1917030 267 motor_rb=0;
Dbee16 7:38def1917030 268 motor_lf=0;
Dbee16 7:38def1917030 269 motor_lb=1; //
cmcmaster 0:59d25bb7f825 270
Dbee16 7:38def1917030 271 wait(2);
Dbee16 7:38def1917030 272 myled = !myled;
cmcmaster 0:59d25bb7f825 273
Dbee16 7:38def1917030 274 motor_rf=0;
Dbee16 7:38def1917030 275 motor_rb=1;
Dbee16 7:38def1917030 276 motor_lf=0;
Dbee16 7:38def1917030 277 motor_lb=0; //reverse turn
cmcmaster 0:59d25bb7f825 278
Dbee16 7:38def1917030 279 wait(2);
cmcmaster 0:59d25bb7f825 280
Dbee16 7:38def1917030 281 myled = !myled;
Dbee16 7:38def1917030 282 motor_r.write(0.5);
Dbee16 7:38def1917030 283 motor_l.write(1.0);
cmcmaster 0:59d25bb7f825 284
Dbee16 7:38def1917030 285 motor_rf=1;
Dbee16 7:38def1917030 286 motor_rb=0;
Dbee16 7:38def1917030 287 motor_lf=1;
Dbee16 7:38def1917030 288 motor_lb=0; //go forwards
Dbee16 7:38def1917030 289 //
Dbee16 7:38def1917030 290 wait(4);
Dbee16 7:38def1917030 291 }*/
Dbee16 7:38def1917030 292
cmcmaster 0:59d25bb7f825 293
cmcmaster 0:59d25bb7f825 294
cmcmaster 0:59d25bb7f825 295 }