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@11:f07662c64e26, 2015-03-13 (annotated)

Committer:
Dbee16
Date:
Fri Mar 13 10:55:39 2015 +0000
Revision:
11:f07662c64e26
Parent:
9:92895704e1a4
Child:
15:3f6626f68836
Ready for motor+blue+sensor Test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cmcmaster 0:59d25bb7f825 1 #include "mbed.h"
cmcmaster 0:59d25bb7f825 2
Dbee16 9:92895704e1a4 3 Serial blue(PTC4,PTC3);
Dbee16 9:92895704e1a4 4
cmcmaster 0:59d25bb7f825 5 //motor select pins
cmcmaster 0:59d25bb7f825 6 DigitalOut motor_lf(PTE2);
cmcmaster 0:59d25bb7f825 7 DigitalOut motor_lb(PTE3);
cmcmaster 0:59d25bb7f825 8 DigitalOut motor_rf(PTE4);
cmcmaster 0:59d25bb7f825 9 DigitalOut motor_rb(PTE5);
cmcmaster 0:59d25bb7f825 10
Dbee16 9:92895704e1a4 11
Dbee16 9:92895704e1a4 12 DigitalIn input(PTC1); //input from sensor array
Dbee16 9:92895704e1a4 13 DigitalOut Line_right(LED_GREEN);//no line detected
Dbee16 9:92895704e1a4 14 //DigitalOut blue(LED_BLUE);
Dbee16 9:92895704e1a4 15
Dbee16 9:92895704e1a4 16 BusOut sensor(PTD7,PTE1,PTE0);//multiplex sensors
Dbee16 9:92895704e1a4 17 AnalogOut level(PTE30);//set comparator level needs to be tuned for each sensor (could create program)
Dbee16 9:92895704e1a4 18
Dbee16 9:92895704e1a4 19
cmcmaster 0:59d25bb7f825 20 //Frequency of Pulse Width Modulated signal in Hz
cmcmaster 0:59d25bb7f825 21 #define PWM_FREQ 1000
cmcmaster 0:59d25bb7f825 22
cmcmaster 0:59d25bb7f825 23 //PWM pin (Enable 1 and 2)
cmcmaster 0:59d25bb7f825 24 PwmOut motor_l (PTC2);
cmcmaster 0:59d25bb7f825 25 PwmOut motor_r (PTE29);
cmcmaster 0:59d25bb7f825 26
cmcmaster 0:59d25bb7f825 27 //LED to test
Dbee16 4:6db8e0babea7 28 DigitalOut led(LED_RED);
Dbee16 1:2bab3a0bc3bc 29
Dbee16 9:92895704e1a4 30 void sensor_select(int x)//converts int to hex for display
cmcmaster 0:59d25bb7f825 31 {
Dbee16 9:92895704e1a4 32 switch(x) {
Dbee16 9:92895704e1a4 33 case 0:
Dbee16 9:92895704e1a4 34 sensor = 0x2;
Dbee16 9:92895704e1a4 35 break;
Dbee16 9:92895704e1a4 36 case 1:
Dbee16 9:92895704e1a4 37 sensor = 0x1;
Dbee16 9:92895704e1a4 38 break;
Dbee16 9:92895704e1a4 39 case 2:
Dbee16 9:92895704e1a4 40 sensor = 0x0;
Dbee16 9:92895704e1a4 41 break;
Dbee16 9:92895704e1a4 42 case 3:
Dbee16 9:92895704e1a4 43 sensor = 0x3;
Dbee16 9:92895704e1a4 44 break;
Dbee16 9:92895704e1a4 45 case 4:
Dbee16 9:92895704e1a4 46 sensor = 0x5;
Dbee16 9:92895704e1a4 47 break;
Dbee16 9:92895704e1a4 48 case 5:
Dbee16 9:92895704e1a4 49 sensor = 0x7;
Dbee16 9:92895704e1a4 50 break;
Dbee16 9:92895704e1a4 51 case 6:
Dbee16 9:92895704e1a4 52 sensor = 0x6;
Dbee16 9:92895704e1a4 53 break;
Dbee16 9:92895704e1a4 54 case 7:
Dbee16 9:92895704e1a4 55 sensor = 0x4;
Dbee16 9:92895704e1a4 56 break;
Dbee16 9:92895704e1a4 57 }
Dbee16 1:2bab3a0bc3bc 58
Dbee16 9:92895704e1a4 59 }
Dbee16 9:92895704e1a4 60 int sensor_read()
Dbee16 9:92895704e1a4 61 {
Dbee16 9:92895704e1a4 62 int val=0;
Dbee16 9:92895704e1a4 63 int x = 0;
Dbee16 1:2bab3a0bc3bc 64
Dbee16 9:92895704e1a4 65 //static int values[8];
Dbee16 9:92895704e1a4 66 for(x = 0; x <= 3; x++ ) { //why 3? Don't you want to select through 8 sensors therefore x<=7?
Dbee16 9:92895704e1a4 67 sensor_select(x);
Dbee16 9:92895704e1a4 68 //blue = !blue;
Dbee16 9:92895704e1a4 69 wait(0.1);
Dbee16 11:f07662c64e26 70 blue.printf("X is %i, ", x);
Dbee16 1:2bab3a0bc3bc 71
Dbee16 9:92895704e1a4 72 if (input == 1) {
Dbee16 9:92895704e1a4 73 val=val+2^x; //could you comment on what this is for?
Dbee16 9:92895704e1a4 74 } else {
Dbee16 9:92895704e1a4 75 val=val;//+2^x;
Dbee16 9:92895704e1a4 76 }
Dbee16 9:92895704e1a4 77 }
Dbee16 11:f07662c64e26 78 blue.printf("\nVAL : %i\n",val);
Dbee16 9:92895704e1a4 79 return val;
Dbee16 9:92895704e1a4 80 }
Dbee16 1:2bab3a0bc3bc 81
Dbee16 9:92895704e1a4 82
Dbee16 9:92895704e1a4 83 void set_direction( int direction, float duty_l, float duty_r)
Dbee16 9:92895704e1a4 84 {
Dbee16 2:1feae3cb6731 85 switch( direction ) {
Dbee16 2:1feae3cb6731 86 case 0x11: { //forward
Dbee16 11:f07662c64e26 87 motor_r.write( duty_r);
Dbee16 2:1feae3cb6731 88 //Look up ternary statements -- {condition ? value_if_true : value_if_false} This makes 0<result<1
Dbee16 9:92895704e1a4 89 motor_l.write( duty_l);
Dbee16 2:1feae3cb6731 90
cmcmaster 0:59d25bb7f825 91 motor_rf=1;
cmcmaster 0:59d25bb7f825 92 motor_rb=0;
cmcmaster 0:59d25bb7f825 93 motor_lf=1;
cmcmaster 0:59d25bb7f825 94 motor_lb=0;
Dbee16 11:f07662c64e26 95 blue.printf("Going forward, right:%f ; left:%f\n", duty_r, duty_l);
Dbee16 11:f07662c64e26 96 break;
cmcmaster 0:59d25bb7f825 97 }
Dbee16 2:1feae3cb6731 98 case 0x00: { //backward
Dbee16 9:92895704e1a4 99 motor_r.write( duty_r);
Dbee16 9:92895704e1a4 100 motor_l.write( duty_l);
Dbee16 2:1feae3cb6731 101
cmcmaster 0:59d25bb7f825 102 motor_rf=0;
cmcmaster 0:59d25bb7f825 103 motor_rb=1;
cmcmaster 0:59d25bb7f825 104 motor_lf=0;
cmcmaster 0:59d25bb7f825 105 motor_lb=1;
Dbee16 11:f07662c64e26 106 blue.printf("Going backward, right:%f ; left:%f\n", duty_r, duty_l);
Dbee16 11:f07662c64e26 107 break;
cmcmaster 0:59d25bb7f825 108 }
Dbee16 2:1feae3cb6731 109 case 0x01: { //spin left -- Right forward, left backward
Dbee16 9:92895704e1a4 110 motor_r.write( duty_r);
Dbee16 9:92895704e1a4 111 motor_l.write( duty_l);
Dbee16 2:1feae3cb6731 112
Dbee16 2:1feae3cb6731 113 motor_rf=1;
Dbee16 2:1feae3cb6731 114 motor_rb=0;
Dbee16 2:1feae3cb6731 115 motor_lf=0;
Dbee16 2:1feae3cb6731 116 motor_lb=1;
Dbee16 11:f07662c64e26 117 blue.printf("Spinning Left, right:%f ; left:%f\n", duty_r, duty_l);
Dbee16 11:f07662c64e26 118 break;
Dbee16 2:1feae3cb6731 119 }
Dbee16 2:1feae3cb6731 120 case 0x10: { //spin right
Dbee16 9:92895704e1a4 121 motor_r.write( duty_r);
Dbee16 9:92895704e1a4 122 motor_l.write( duty_l);
Dbee16 2:1feae3cb6731 123
Dbee16 2:1feae3cb6731 124 motor_rf=0;
Dbee16 2:1feae3cb6731 125 motor_rb=1;
Dbee16 2:1feae3cb6731 126 motor_lf=1;
Dbee16 2:1feae3cb6731 127 motor_lb=0;
Dbee16 11:f07662c64e26 128 blue.printf("Spinning Right, right:%f ; left:%f\n", duty_r, duty_l);
Dbee16 11:f07662c64e26 129 break;
Dbee16 2:1feae3cb6731 130 }
cmcmaster 0:59d25bb7f825 131 }
cmcmaster 0:59d25bb7f825 132 }
Dbee16 9:92895704e1a4 133
Dbee16 9:92895704e1a4 134 void change_direction( int sensor_values) //right hand sensors only
Dbee16 8:eefbd3880d28 135 {
Dbee16 9:92895704e1a4 136 //Line_right = !Line_right;
Dbee16 9:92895704e1a4 137 switch( sensor_values ) {
Dbee16 9:92895704e1a4 138 case 0x00: { //no lines robot is lost!!!!
Dbee16 11:f07662c64e26 139 blue.printf("no lines robot is lost!!!!\n");
Dbee16 9:92895704e1a4 140 set_direction(0x00, 1.0, 1.0);
Dbee16 9:92895704e1a4 141 wait(1);
Dbee16 9:92895704e1a4 142 break;
Dbee16 9:92895704e1a4 143 }
Dbee16 9:92895704e1a4 144 case 0x01: { //to far left turn right 0001
Dbee16 11:f07662c64e26 145 blue.printf("0001\n");
Dbee16 9:92895704e1a4 146 set_direction(0x11, 0.0, 1.0); //this doesn't turn right; this goes straight at half speed
Dbee16 9:92895704e1a4 147 wait(1);
Dbee16 9:92895704e1a4 148 Line_right = !Line_right;
Dbee16 9:92895704e1a4 149 break;
Dbee16 9:92895704e1a4 150 }
Dbee16 9:92895704e1a4 151 case 0x02: { //left turn right 0010
Dbee16 11:f07662c64e26 152 blue.printf("0010\n");
Dbee16 9:92895704e1a4 153 set_direction(0x11, 0, 0.5);
Dbee16 9:92895704e1a4 154 wait(1);
Dbee16 9:92895704e1a4 155 break;
Dbee16 9:92895704e1a4 156 }
Dbee16 9:92895704e1a4 157 case 0x03: { //far left turn right 0011
Dbee16 11:f07662c64e26 158 blue.printf("0011\n");
Dbee16 9:92895704e1a4 159 set_direction(0x11, 0.0, 0.0);
Dbee16 9:92895704e1a4 160 wait(1);
Dbee16 9:92895704e1a4 161 break;
Dbee16 9:92895704e1a4 162 }
Dbee16 9:92895704e1a4 163 case 0x4: { //right turn left 0100
Dbee16 11:f07662c64e26 164 blue.printf("0100\n");
Dbee16 9:92895704e1a4 165 set_direction(0x11, 0.5, 0.0);
Dbee16 9:92895704e1a4 166 wait(1);
Dbee16 9:92895704e1a4 167 break;
Dbee16 9:92895704e1a4 168 }
Dbee16 9:92895704e1a4 169 case 0x5: { //unknown ** 0101
Dbee16 9:92895704e1a4 170 set_direction(0x11, 0.0, 0.0);
Dbee16 9:92895704e1a4 171 break;
Dbee16 9:92895704e1a4 172 }
Dbee16 9:92895704e1a4 173 case 0x6: { //perfect 0110
Dbee16 11:f07662c64e26 174 blue.printf("perfect, 0110\n");
Dbee16 9:92895704e1a4 175 set_direction(0x00, 1.0, 1.0);
Dbee16 9:92895704e1a4 176 wait(1);
Dbee16 9:92895704e1a4 177 break;
Dbee16 9:92895704e1a4 178 }
Dbee16 9:92895704e1a4 179 case 0x7: {//corner junction or plate 0111
Dbee16 11:f07662c64e26 180 blue.printf("0111\n");
Dbee16 9:92895704e1a4 181 set_direction(0x00, 0.0, 0.0);
Dbee16 9:92895704e1a4 182 wait(1);
Dbee16 9:92895704e1a4 183 break;
Dbee16 9:92895704e1a4 184 }
Dbee16 9:92895704e1a4 185 case 0x8: {//to far right turn left 1000
Dbee16 11:f07662c64e26 186 blue.printf("1000\n");
Dbee16 9:92895704e1a4 187 set_direction(0x11, 1.0, 0.0);
Dbee16 9:92895704e1a4 188 wait(1);
Dbee16 9:92895704e1a4 189 break;
Dbee16 9:92895704e1a4 190 }
Dbee16 9:92895704e1a4 191 case 0x9: { //unknown 1001
Dbee16 9:92895704e1a4 192 set_direction(0x11, 0.0, 0.0);
Dbee16 9:92895704e1a4 193 wait(1);
Dbee16 9:92895704e1a4 194 break;
Dbee16 9:92895704e1a4 195 }
Dbee16 9:92895704e1a4 196 case 0xA: { //unknown 1010
Dbee16 9:92895704e1a4 197 set_direction(0x11, 0.0, 0.0);
Dbee16 9:92895704e1a4 198 break;
Dbee16 9:92895704e1a4 199 }
Dbee16 9:92895704e1a4 200 case 0xB: { //unknown 1011
Dbee16 9:92895704e1a4 201 set_direction(0x11, 0.0, 0.0);
Dbee16 9:92895704e1a4 202 wait(1);
Dbee16 9:92895704e1a4 203 break;
Dbee16 9:92895704e1a4 204 }
Dbee16 9:92895704e1a4 205 case 0xC: { //far right turn left 1100
Dbee16 11:f07662c64e26 206 blue.printf("1100\n");
Dbee16 9:92895704e1a4 207 set_direction(0x00, 0.0, 0.0);
Dbee16 9:92895704e1a4 208 wait(1);
Dbee16 9:92895704e1a4 209 break;
Dbee16 9:92895704e1a4 210 }
Dbee16 9:92895704e1a4 211 case 0xD: { //unknown 1101
Dbee16 9:92895704e1a4 212 set_direction(0x11, 0.0, 0.0);
Dbee16 9:92895704e1a4 213 break;
Dbee16 9:92895704e1a4 214 }
Dbee16 9:92895704e1a4 215 case 0xE: { //corner or plate 1110
Dbee16 11:f07662c64e26 216 blue.printf("1110\n");
Dbee16 9:92895704e1a4 217 set_direction(0x00, 0.0, 0.0);
Dbee16 9:92895704e1a4 218 wait(1);
Dbee16 9:92895704e1a4 219 break;
Dbee16 9:92895704e1a4 220 }
Dbee16 9:92895704e1a4 221 case 0xF: { //corner 1111
Dbee16 11:f07662c64e26 222 blue.printf("1111\n");
Dbee16 9:92895704e1a4 223 set_direction(0x11, 0.0, 0.0);
Dbee16 9:92895704e1a4 224 wait(1);
Dbee16 9:92895704e1a4 225 break;
Dbee16 9:92895704e1a4 226 }
Dbee16 8:eefbd3880d28 227 }
Dbee16 8:eefbd3880d28 228 }
cmcmaster 0:59d25bb7f825 229 int main()
cmcmaster 0:59d25bb7f825 230 {
Dbee16 9:92895704e1a4 231 level = 0.3; //Analogout Level for black line into comparator
Dbee16 2:1feae3cb6731 232 //Set PWM frequency to 1000Hz
cmcmaster 0:59d25bb7f825 233 motor_l.period( 1.0f / (float) PWM_FREQ);
cmcmaster 0:59d25bb7f825 234 motor_r.period( 1.0f / (float) PWM_FREQ);
cmcmaster 0:59d25bb7f825 235 //Initialise direction to nothing.
cmcmaster 0:59d25bb7f825 236 motor_rf=0;
cmcmaster 0:59d25bb7f825 237 motor_rb=0;
cmcmaster 0:59d25bb7f825 238 motor_lf=0;
cmcmaster 0:59d25bb7f825 239 motor_lb=0;
cmcmaster 0:59d25bb7f825 240
Dbee16 9:92895704e1a4 241 while(1) {
Dbee16 11:f07662c64e26 242 //Working
Dbee16 11:f07662c64e26 243 /*
Dbee16 11:f07662c64e26 244 led = !led;
Dbee16 11:f07662c64e26 245 wait(4);
Dbee16 11:f07662c64e26 246 blue.printf("Forward, full\n");
Dbee16 11:f07662c64e26 247 set_direction(0x11, 1.0,1.0);//forward full speed
Dbee16 11:f07662c64e26 248 wait(4);
Dbee16 11:f07662c64e26 249 blue.printf("Forward, left half\n");
Dbee16 11:f07662c64e26 250 led = !led;
Dbee16 11:f07662c64e26 251 set_direction(0x11, 0.5 ,0); //forward left half
Dbee16 11:f07662c64e26 252 wait(4);
Dbee16 11:f07662c64e26 253 blue.printf("Forward, right half\n");
Dbee16 11:f07662c64e26 254 led = !led;
Dbee16 11:f07662c64e26 255 set_direction(0x11, 0 ,0.5); //forward right half
Dbee16 11:f07662c64e26 256 wait(4);
Dbee16 11:f07662c64e26 257 led = !led;
Dbee16 11:f07662c64e26 258 blue.printf("stop\n");
Dbee16 11:f07662c64e26 259 set_direction(0x00, 0 ,0); //stop
Dbee16 11:f07662c64e26 260 wait(4);
Dbee16 11:f07662c64e26 261 led = !led;
Dbee16 11:f07662c64e26 262 blue.printf("Back, full\n");
Dbee16 11:f07662c64e26 263 set_direction(0x00, 1.0 ,1.0); //backward full speed
Dbee16 11:f07662c64e26 264 wait(4);
Dbee16 11:f07662c64e26 265 led = !led;
Dbee16 11:f07662c64e26 266 blue.printf("Back, left\n");
Dbee16 11:f07662c64e26 267 set_direction(0x00, 0.5 ,0);
Dbee16 11:f07662c64e26 268 wait(4);
Dbee16 11:f07662c64e26 269 blue.printf("Back, right\n");
Dbee16 11:f07662c64e26 270 led = !led;
Dbee16 11:f07662c64e26 271 set_direction(0x00, 0 ,0.5);
Dbee16 11:f07662c64e26 272
Dbee16 9:92895704e1a4 273 */
Dbee16 11:f07662c64e26 274
Dbee16 9:92895704e1a4 275 //Sensor Code + Motor Test
Dbee16 11:f07662c64e26 276 int values = 0;
Dbee16 11:f07662c64e26 277 values = sensor_read();
Dbee16 11:f07662c64e26 278 change_direction(values);
Dbee16 9:92895704e1a4 279
Dbee16 9:92895704e1a4 280 }
Dbee16 9:92895704e1a4 281 }
Dbee16 8:eefbd3880d28 282
Dbee16 8:eefbd3880d28 283
Dbee16 8:eefbd3880d28 284
Dbee16 8:eefbd3880d28 285
Dbee16 8:eefbd3880d28 286