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@21:bef848ac1894, 2015-03-25 (annotated)

Committer:
Dbee16
Date:
Wed Mar 25 11:24:16 2015 +0000
Revision:
21:bef848ac1894
Parent:
20:823f05f5ffd5 
still need line snesore test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cmcmaster 0:59d25bb7f825 1 #include "mbed.h"
Dbee16 17:54da4359134f 2 #include <string>
Dbee16 17:54da4359134f 3 using namespace std;
Dbee16 9:92895704e1a4 4 Serial blue(PTC4,PTC3);
Dbee16 9:92895704e1a4 5
cmcmaster 0:59d25bb7f825 6 //motor select pins
cmcmaster 0:59d25bb7f825 7 DigitalOut motor_lf(PTE2);
cmcmaster 0:59d25bb7f825 8 DigitalOut motor_lb(PTE3);
cmcmaster 0:59d25bb7f825 9 DigitalOut motor_rf(PTE4);
cmcmaster 0:59d25bb7f825 10 DigitalOut motor_rb(PTE5);
cmcmaster 0:59d25bb7f825 11
Dbee16 9:92895704e1a4 12
Dbee16 9:92895704e1a4 13 DigitalIn input(PTC1); //input from sensor array
cmcmaster 15:3f6626f68836 14 //DigitalOut Line_right(LED_GREEN);//no line detected
Dbee16 9:92895704e1a4 15 //DigitalOut blue(LED_BLUE);
Dbee16 9:92895704e1a4 16
Dbee16 9:92895704e1a4 17 BusOut sensor(PTD7,PTE1,PTE0);//multiplex sensors
Dbee16 9:92895704e1a4 18 AnalogOut level(PTE30);//set comparator level needs to be tuned for each sensor (could create program)
Dbee16 9:92895704e1a4 19
Dbee16 9:92895704e1a4 20
cmcmaster 0:59d25bb7f825 21 //Frequency of Pulse Width Modulated signal in Hz
cmcmaster 0:59d25bb7f825 22 #define PWM_FREQ 1000
cmcmaster 0:59d25bb7f825 23
cmcmaster 0:59d25bb7f825 24 //PWM pin (Enable 1 and 2)
cmcmaster 0:59d25bb7f825 25 PwmOut motor_l (PTC2);
cmcmaster 0:59d25bb7f825 26 PwmOut motor_r (PTE29);
cmcmaster 0:59d25bb7f825 27
cmcmaster 0:59d25bb7f825 28 //LED to test
Dbee16 4:6db8e0babea7 29 DigitalOut led(LED_RED);
Dbee16 1:2bab3a0bc3bc 30
Dbee16 18:6e8eae661cf3 31 //integer value of sensors (has to be global)
Dbee16 18:6e8eae661cf3 32 int val_r=0;
Dbee16 18:6e8eae661cf3 33 int val_l=0;
Dbee16 18:6e8eae661cf3 34 int old_val_r=0;
Dbee16 18:6e8eae661cf3 35 int old_val_l=0;
Dbee16 1:2bab3a0bc3bc 36
Dbee16 18:6e8eae661cf3 37 //Flags for decisions
Dbee16 20:823f05f5ffd5 38 int l_turn_f = 0;
Dbee16 20:823f05f5ffd5 39 int r_turn_f = 0;
Dbee16 20:823f05f5ffd5 40 int l_180_f = 0;
Dbee16 20:823f05f5ffd5 41 int r_180_f = 0;
Dbee16 21:bef848ac1894 42 int forward = 0;
Dbee16 18:6e8eae661cf3 43 int backwards = 0;
Dbee16 18:6e8eae661cf3 44 int spin = 0;
Dbee16 18:6e8eae661cf3 45 //Junction Counter (hard coding deccsions on junctions)
Dbee16 18:6e8eae661cf3 46 int junction_cnt = 0;
Dbee16 9:92895704e1a4 47
Dbee16 9:92895704e1a4 48 void set_direction( int direction, float duty_l, float duty_r)
Dbee16 9:92895704e1a4 49 {
Dbee16 20:823f05f5ffd5 50 blue.printf("Change in direction; %f, %f \n", duty_l, duty_r);
Dbee16 2:1feae3cb6731 51 switch( direction ) {
Dbee16 2:1feae3cb6731 52 case 0x11: { //forward
Dbee16 11:f07662c64e26 53 motor_r.write( duty_r);
Dbee16 2:1feae3cb6731 54 //Look up ternary statements -- {condition ? value_if_true : value_if_false} This makes 0<result<1
Dbee16 9:92895704e1a4 55 motor_l.write( duty_l);
Dbee16 2:1feae3cb6731 56
cmcmaster 0:59d25bb7f825 57 motor_rf=1;
cmcmaster 0:59d25bb7f825 58 motor_rb=0;
cmcmaster 0:59d25bb7f825 59 motor_lf=1;
cmcmaster 0:59d25bb7f825 60 motor_lb=0;
Dbee16 17:54da4359134f 61 // blue.printf("Going forward, right:%f ; left:%f\n", duty_r, duty_l);
Dbee16 11:f07662c64e26 62 break;
cmcmaster 0:59d25bb7f825 63 }
Dbee16 2:1feae3cb6731 64 case 0x00: { //backward
Dbee16 9:92895704e1a4 65 motor_r.write( duty_r);
Dbee16 9:92895704e1a4 66 motor_l.write( duty_l);
Dbee16 2:1feae3cb6731 67
cmcmaster 0:59d25bb7f825 68 motor_rf=0;
cmcmaster 0:59d25bb7f825 69 motor_rb=1;
cmcmaster 0:59d25bb7f825 70 motor_lf=0;
cmcmaster 0:59d25bb7f825 71 motor_lb=1;
Dbee16 17:54da4359134f 72 // blue.printf("Going backward, right:%f ; left:%f\n", duty_r, duty_l);
Dbee16 11:f07662c64e26 73 break;
cmcmaster 0:59d25bb7f825 74 }
Dbee16 2:1feae3cb6731 75 case 0x01: { //spin left -- Right forward, left backward
Dbee16 9:92895704e1a4 76 motor_r.write( duty_r);
Dbee16 9:92895704e1a4 77 motor_l.write( duty_l);
Dbee16 2:1feae3cb6731 78
Dbee16 2:1feae3cb6731 79 motor_rf=1;
Dbee16 2:1feae3cb6731 80 motor_rb=0;
Dbee16 2:1feae3cb6731 81 motor_lf=0;
Dbee16 2:1feae3cb6731 82 motor_lb=1;
Dbee16 17:54da4359134f 83 // blue.printf("Spinning Left, right:%f ; left:%f\n", duty_r, duty_l);
Dbee16 11:f07662c64e26 84 break;
Dbee16 2:1feae3cb6731 85 }
Dbee16 2:1feae3cb6731 86 case 0x10: { //spin right
Dbee16 9:92895704e1a4 87 motor_r.write( duty_r);
Dbee16 9:92895704e1a4 88 motor_l.write( duty_l);
Dbee16 2:1feae3cb6731 89
Dbee16 2:1feae3cb6731 90 motor_rf=0;
Dbee16 2:1feae3cb6731 91 motor_rb=1;
Dbee16 2:1feae3cb6731 92 motor_lf=1;
Dbee16 2:1feae3cb6731 93 motor_lb=0;
Dbee16 17:54da4359134f 94 // blue.printf("Spinning Right, right:%f ; left:%f\n", duty_r, duty_l);
Dbee16 11:f07662c64e26 95 break;
Dbee16 2:1feae3cb6731 96 }
cmcmaster 0:59d25bb7f825 97 }
cmcmaster 0:59d25bb7f825 98 }
Dbee16 20:823f05f5ffd5 99 int change =0;
Dbee16 9:92895704e1a4 100
Dbee16 20:823f05f5ffd5 101 void r_normal()
Dbee16 8:eefbd3880d28 102 {
Dbee16 20:823f05f5ffd5 103 // blue.printf("0110\n");
Dbee16 20:823f05f5ffd5 104 if (val_l == 0) {
Dbee16 20:823f05f5ffd5 105 //check flags
Dbee16 20:823f05f5ffd5 106 if( l_180_f ) {
Dbee16 20:823f05f5ffd5 107 set_direction(0x11, 0.2, 0.6); //Needs to test
Dbee16 20:823f05f5ffd5 108 spin =1; //new state is spin
Dbee16 21:bef848ac1894 109 forward = 0;
Dbee16 20:823f05f5ffd5 110 //start_timer?
Dbee16 20:823f05f5ffd5 111
Dbee16 20:823f05f5ffd5 112 } else if (l_turn_f) { //
Dbee16 20:823f05f5ffd5 113 set_direction(0x01, 0.3,0.3); //CHANGE DUTIES AFTER TESTING
Dbee16 20:823f05f5ffd5 114 spin =1 ; //reached the end, nothign else matters
Dbee16 21:bef848ac1894 115 forward =0;
Dbee16 20:823f05f5ffd5 116 }
Dbee16 20:823f05f5ffd5 117 } else if(val_l == 7) {
Dbee16 20:823f05f5ffd5 118 l_turn_f =1;
Dbee16 21:bef848ac1894 119 if(!forward) {
Dbee16 21:bef848ac1894 120 forward =1;
Dbee16 20:823f05f5ffd5 121 set_direction(0x11, 0.3,0.3);
Dbee16 21:bef848ac1894 122 } else if(!forward) {
Dbee16 21:bef848ac1894 123 forward =1;
Dbee16 20:823f05f5ffd5 124
Dbee16 20:823f05f5ffd5 125 set_direction(0x11, 0.3,0.3);
Dbee16 20:823f05f5ffd5 126 }
Dbee16 20:823f05f5ffd5 127 }
Dbee16 20:823f05f5ffd5 128
Dbee16 20:823f05f5ffd5 129
Dbee16 20:823f05f5ffd5 130 }
Dbee16 20:823f05f5ffd5 131 void motor_result() //val_r and val_l are global
Dbee16 20:823f05f5ffd5 132 {
Dbee16 20:823f05f5ffd5 133 switch (val_r) {
Dbee16 18:6e8eae661cf3 134 case 6: //Normal Mode
Dbee16 20:823f05f5ffd5 135 r_normal();
Dbee16 18:6e8eae661cf3 136 break;
Dbee16 20:823f05f5ffd5 137 case 7: // 0111
Dbee16 20:823f05f5ffd5 138 r_turn_f =1; //Either misaligned or getting ready for a turn
Dbee16 20:823f05f5ffd5 139 if (val_l == 14) { //1110 0111
Dbee16 20:823f05f5ffd5 140 l_turn_f =1;
Dbee16 20:823f05f5ffd5 141 } else if(val_l ==0) { //0000 0111
Dbee16 20:823f05f5ffd5 142 if (l_turn_f) { //special case ****************************** WON'T HAPPEN**********************
Dbee16 20:823f05f5ffd5 143 blue.printf("Double Turn?");
Dbee16 20:823f05f5ffd5 144 set_direction(0x01, 0.3,0.3);
Dbee16 21:bef848ac1894 145 forward =0;
Dbee16 20:823f05f5ffd5 146 spin =1;
Dbee16 20:823f05f5ffd5 147 } else if(l_180_f) {
Dbee16 20:823f05f5ffd5 148 blue.printf("You've got a junction\n");
Dbee16 20:823f05f5ffd5 149 } else {
Dbee16 20:823f05f5ffd5 150 l_180_f =1;
Dbee16 20:823f05f5ffd5 151 //No need to change direction, if misaligned, it will soon go to a double line alignment protocol
Dbee16 20:823f05f5ffd5 152 //if the next state is 0000; then we were right and there is a right turn.
Dbee16 20:823f05f5ffd5 153 }
Dbee16 18:6e8eae661cf3 154 }
cmcmaster 15:3f6626f68836 155 break;
Dbee16 18:6e8eae661cf3 156
Dbee16 18:6e8eae661cf3 157
Dbee16 20:823f05f5ffd5 158 case 0: //lost or turn
Dbee16 20:823f05f5ffd5 159 // blue.printf("0000\n");
Dbee16 20:823f05f5ffd5 160 if(val_l == 14) {
Dbee16 20:823f05f5ffd5 161 //This probably won't happen, but if it does , 180
Dbee16 20:823f05f5ffd5 162 r_180_f =1;
Dbee16 20:823f05f5ffd5 163 l_turn_f =1;
Dbee16 20:823f05f5ffd5 164 } else if(val_l == 0) {
Dbee16 20:823f05f5ffd5 165 switch (junction_cnt) {
Dbee16 20:823f05f5ffd5 166 default:
Dbee16 20:823f05f5ffd5 167 set_direction(0x11, 0,0);
Dbee16 20:823f05f5ffd5 168 forward = 0;
Dbee16 20:823f05f5ffd5 169 }
Dbee16 20:823f05f5ffd5 170 } else if(r_turn_f) {
Dbee16 21:bef848ac1894 171 set_direction(0x10, 0.3 ,0.3);
Dbee16 20:823f05f5ffd5 172 forward = 0;
Dbee16 20:823f05f5ffd5 173 spin = 1;
Dbee16 21:bef848ac1894 174 } else if (r_180_f) {
Dbee16 20:823f05f5ffd5 175 set_direction(0x11, 0.6, 0.2);
Dbee16 20:823f05f5ffd5 176 forward = 0;
Dbee16 20:823f05f5ffd5 177 spin =1;
Dbee16 21:bef848ac1894 178 } else {
Dbee16 20:823f05f5ffd5 179 r_180_f =1;
Dbee16 20:823f05f5ffd5 180 }
Dbee16 9:92895704e1a4 181 break;
Dbee16 20:823f05f5ffd5 182
Dbee16 20:823f05f5ffd5 183
cmcmaster 15:3f6626f68836 184 case 2:
Dbee16 17:54da4359134f 185 // blue.printf("0010\n");
Dbee16 20:823f05f5ffd5 186 r_normal();
cmcmaster 15:3f6626f68836 187 break;
cmcmaster 15:3f6626f68836 188 case 4:
Dbee16 17:54da4359134f 189 // blue.printf("0100\n");
Dbee16 20:823f05f5ffd5 190 r_normal();
Dbee16 9:92895704e1a4 191 break;
Dbee16 20:823f05f5ffd5 192
Dbee16 20:823f05f5ffd5 193 //***************************IF THERE IS A SINGLE BIT ONLY; WE ARE very MISALIGNED*********************************
Dbee16 20:823f05f5ffd5 194 case 1:
Dbee16 20:823f05f5ffd5 195 // blue.printf("0001\n");
Dbee16 20:823f05f5ffd5 196 set_direction(0x11, 0,0);
Dbee16 20:823f05f5ffd5 197 forward=0;
Dbee16 20:823f05f5ffd5 198 break;
Dbee16 20:823f05f5ffd5 199
Dbee16 18:6e8eae661cf3 200 case 8:
Dbee16 18:6e8eae661cf3 201 //blue.printf("1000\n");
Dbee16 20:823f05f5ffd5 202 set_direction(0x11, 0,0);
Dbee16 20:823f05f5ffd5 203 forward = 0;
Dbee16 17:54da4359134f 204 break;
Dbee16 18:6e8eae661cf3 205 //******************************END OF SINGLE BIT ALIGNMENT********************************************
Dbee16 18:6e8eae661cf3 206
Dbee16 18:6e8eae661cf3 207 //*************IF TWO BITS ARE ON, AND NOT IN THE MIDDLE, WE'RE PROBABLY VERY MISALIGNED*******************
Dbee16 17:54da4359134f 208 case 12:
Dbee16 17:54da4359134f 209 // blue.printf("1100\n");
Dbee16 17:54da4359134f 210 set_direction(0x11, 0.2,0.3);
Dbee16 20:823f05f5ffd5 211 forward = 0;
Dbee16 17:54da4359134f 212 break;
Dbee16 17:54da4359134f 213 case 3:
Dbee16 17:54da4359134f 214 // blue.printf("0011\n");
Dbee16 17:54da4359134f 215 set_direction(0x11, 0.3,0.2);
Dbee16 20:823f05f5ffd5 216 forward=0;
cmcmaster 15:3f6626f68836 217 break;
Dbee16 18:6e8eae661cf3 218 //********************************END OF DOUBLE BIT ALIGNMENT********************************************
Dbee16 18:6e8eae661cf3 219
Dbee16 20:823f05f5ffd5 220 case 15:
Dbee16 20:823f05f5ffd5 221 set_direction(0x11, 0,0);
Dbee16 20:823f05f5ffd5 222 forward =0;
Dbee16 20:823f05f5ffd5 223 break;
Dbee16 18:6e8eae661cf3 224
Dbee16 18:6e8eae661cf3 225 default:
Dbee16 18:6e8eae661cf3 226 //blue.printf("%i\n", val);
Dbee16 18:6e8eae661cf3 227 set_direction(0x00, 0,0);
Dbee16 18:6e8eae661cf3 228 }
Dbee16 20:823f05f5ffd5 229 }
Dbee16 18:6e8eae661cf3 230
Dbee16 21:bef848ac1894 231
Dbee16 20:823f05f5ffd5 232 /*
Dbee16 20:823f05f5ffd5 233 //This needed to be completely changed. We need sequential actions, a case statement is too basic (unforetuneately)
Dbee16 20:823f05f5ffd5 234 led= 0;
Dbee16 20:823f05f5ffd5 235 switch(val_r) {
Dbee16 20:823f05f5ffd5 236 case 4: //Normal mode
Dbee16 21:bef848ac1894 237 if (!forward) {
Dbee16 20:823f05f5ffd5 238 blue.printf("i've got %i on the right\n", val_r);
Dbee16 20:823f05f5ffd5 239 set_direction(0x11, 0.3,0.3);
Dbee16 21:bef848ac1894 240 forward =1;
Dbee16 20:823f05f5ffd5 241 change =1;
Dbee16 20:823f05f5ffd5 242 break;
Dbee16 20:823f05f5ffd5 243 }
Dbee16 20:823f05f5ffd5 244 case 12:
Dbee16 20:823f05f5ffd5 245 blue.printf("i've got %i on the right\n", val_r);
Dbee16 18:6e8eae661cf3 246
Dbee16 20:823f05f5ffd5 247 set_direction(0x11, 0.3, 0.2);
Dbee16 21:bef848ac1894 248 forward =0;
Dbee16 20:823f05f5ffd5 249 break;
Dbee16 20:823f05f5ffd5 250 change =1;
Dbee16 18:6e8eae661cf3 251
Dbee16 20:823f05f5ffd5 252 case 8:
Dbee16 20:823f05f5ffd5 253 blue.printf("i've got %i on the right\n", val_r);
Dbee16 20:823f05f5ffd5 254
Dbee16 20:823f05f5ffd5 255 set_direction(0x11, 0.2, 0.3);
Dbee16 21:bef848ac1894 256 forward =0;
Dbee16 20:823f05f5ffd5 257 change =1;
Dbee16 18:6e8eae661cf3 258
Dbee16 20:823f05f5ffd5 259 break;
Dbee16 20:823f05f5ffd5 260 default:
Dbee16 21:bef848ac1894 261 if (!forward) {
Dbee16 20:823f05f5ffd5 262 blue.printf("i've got %i on the right\n", val_r);
Dbee16 20:823f05f5ffd5 263
Dbee16 20:823f05f5ffd5 264 set_direction(0x11, 0.3,0.3);
Dbee16 21:bef848ac1894 265 forward =1;
Dbee16 20:823f05f5ffd5 266 break;
Dbee16 20:823f05f5ffd5 267 }
Dbee16 20:823f05f5ffd5 268
Dbee16 20:823f05f5ffd5 269 }
Dbee16 20:823f05f5ffd5 270 if(!change) {
Dbee16 20:823f05f5ffd5 271 switch(val_l) {
Dbee16 20:823f05f5ffd5 272 case 2:
Dbee16 21:bef848ac1894 273 if (!forward) {
Dbee16 20:823f05f5ffd5 274 blue.printf("i've got %i on the left\n", val_l);
Dbee16 18:6e8eae661cf3 275
Dbee16 20:823f05f5ffd5 276 set_direction(0x11, 0.3,0.3);
Dbee16 21:bef848ac1894 277 forward =1;
Dbee16 20:823f05f5ffd5 278 break;
Dbee16 20:823f05f5ffd5 279 }
Dbee16 20:823f05f5ffd5 280 case 3:
Dbee16 20:823f05f5ffd5 281 blue.printf("i've got %i on the left\n", val_l);
Dbee16 18:6e8eae661cf3 282
Dbee16 20:823f05f5ffd5 283 set_direction(0x11, 0.3, 0.2);
Dbee16 21:bef848ac1894 284 forward =0;
Dbee16 20:823f05f5ffd5 285 break;
Dbee16 20:823f05f5ffd5 286 case 1:
Dbee16 20:823f05f5ffd5 287 blue.printf("i've got %i on the left\n", val_l);
Dbee16 20:823f05f5ffd5 288
Dbee16 20:823f05f5ffd5 289 set_direction(0x11, 0.3, 0.2);
Dbee16 21:bef848ac1894 290 forward =0;
Dbee16 20:823f05f5ffd5 291 break;
Dbee16 18:6e8eae661cf3 292
Dbee16 18:6e8eae661cf3 293
Dbee16 18:6e8eae661cf3 294
Dbee16 20:823f05f5ffd5 295 default:
Dbee16 20:823f05f5ffd5 296 blue.printf("i've got %i on the left\n", val_l);
Dbee16 21:bef848ac1894 297 if (!forward) {
Dbee16 20:823f05f5ffd5 298 // blue.printf("i've got %i on the left\n", val_l);
Dbee16 20:823f05f5ffd5 299
Dbee16 20:823f05f5ffd5 300 set_direction(0x11, 0.3,0.3);
Dbee16 21:bef848ac1894 301 forward =1;
Dbee16 20:823f05f5ffd5 302 break;
Dbee16 20:823f05f5ffd5 303 }
Dbee16 20:823f05f5ffd5 304 }
Dbee16 20:823f05f5ffd5 305 }*/
Dbee16 20:823f05f5ffd5 306 //********************************************************START******************************************************
Dbee16 20:823f05f5ffd5 307
Dbee16 20:823f05f5ffd5 308
Dbee16 20:823f05f5ffd5 309 //***************************************LEFT SHOULD BE PRETTY MUCH MIRRORED***********************************************************
Dbee16 20:823f05f5ffd5 310 /* switch(val_l) {
Dbee16 20:823f05f5ffd5 311 case 7: //Normal Mode
Dbee16 20:823f05f5ffd5 312 // blue.printf("0110\n");
Dbee16 20:823f05f5ffd5 313 set_direction(0x11, 0.3,0.3);
Dbee16 20:823f05f5ffd5 314 break;
Dbee16 20:823f05f5ffd5 315 // case 7: // 0111 Either misaligned or getting ready for a turn
Dbee16 20:823f05f5ffd5 316 // l_turn_f = 1;
Dbee16 20:823f05f5ffd5 317 //No need to change direction, if misaligned, it will soon go to a double line alignment protocol
Dbee16 20:823f05f5ffd5 318 //if the next state is 0000; then we were right and there is a left turn.
Dbee16 20:823f05f5ffd5 319 // break;
Dbee16 20:823f05f5ffd5 320 case 0: //lost or turn
Dbee16 20:823f05f5ffd5 321 // blue.printf("0000\n");
Dbee16 20:823f05f5ffd5 322 if (r_turn_f & l_turn_f) { //if we're expecting a junction
Dbee16 20:823f05f5ffd5 323 set_direction(0x10, 0.3,0.3); //spin right because there's only one junction
Dbee16 20:823f05f5ffd5 324 //no stop conditions yet
Dbee16 20:823f05f5ffd5 325
Dbee16 20:823f05f5ffd5 326 } else if (l_turn_f) { //if we're expecting a left turn
Dbee16 20:823f05f5ffd5 327 set_direction(0x01, 0.3,0.3); //spin left
Dbee16 20:823f05f5ffd5 328 //no stop conditions yet
Dbee16 20:823f05f5ffd5 329
Dbee16 20:823f05f5ffd5 330 } else if(val_r == 6) { //0000 0110 could mean a left 180 turn
Dbee16 20:823f05f5ffd5 331 //keep going forward, no need to change dir
Dbee16 20:823f05f5ffd5 332 l_180_f = 1; //raise 180 flag in prep for next time
cmcmaster 15:3f6626f68836 333
Dbee16 20:823f05f5ffd5 334 } else if(l_180_f) {
Dbee16 20:823f05f5ffd5 335 //code for 180???
Dbee16 20:823f05f5ffd5 336 blue.printf("I want to 180 left, but i don't know how yet");
Dbee16 20:823f05f5ffd5 337 set_direction(0x00, 0,0);
Dbee16 20:823f05f5ffd5 338
Dbee16 20:823f05f5ffd5 339 } else { //probably lost
Dbee16 20:823f05f5ffd5 340 set_direction(0x00, 0,0);
Dbee16 20:823f05f5ffd5 341 }
Dbee16 20:823f05f5ffd5 342 break;
Dbee16 20:823f05f5ffd5 343
Dbee16 20:823f05f5ffd5 344 //***************************IF THERE IS A SINGLE BIT ONLY; WE ARE MISALIGNED*********************************
Dbee16 20:823f05f5ffd5 345 case 1:
Dbee16 20:823f05f5ffd5 346 // blue.printf("0001\n");
Dbee16 20:823f05f5ffd5 347 set_direction(0x11, 0.1,0.3);
Dbee16 20:823f05f5ffd5 348 break;
Dbee16 20:823f05f5ffd5 349 case 2:
Dbee16 20:823f05f5ffd5 350 // blue.printf("0010\n");
Dbee16 20:823f05f5ffd5 351 set_direction(0x11, 0.2,0.3);
Dbee16 20:823f05f5ffd5 352 break;
Dbee16 20:823f05f5ffd5 353 case 4:
Dbee16 20:823f05f5ffd5 354 // blue.printf("0100\n");
Dbee16 20:823f05f5ffd5 355 set_direction(0x11, 0.3,0.2);
Dbee16 20:823f05f5ffd5 356 break;
Dbee16 20:823f05f5ffd5 357 case 8:
Dbee16 20:823f05f5ffd5 358 //blue.printf("1000\n");
Dbee16 20:823f05f5ffd5 359 set_direction(0x11, 0.3,0.1);
Dbee16 20:823f05f5ffd5 360 break;
Dbee16 20:823f05f5ffd5 361 //******************************END OF SINGLE BIT ALIGNMENT********************************************
Dbee16 20:823f05f5ffd5 362
Dbee16 20:823f05f5ffd5 363 //*************IF TWO BITS ARE ON, AND NOT IN THE MIDDLE, WE'RE PROBABLY VERY MISALIGNED*******************
Dbee16 20:823f05f5ffd5 364 case 12:
Dbee16 20:823f05f5ffd5 365 // blue.printf("1100\n");
Dbee16 20:823f05f5ffd5 366 set_direction(0x11, 0.3,0.2);
Dbee16 20:823f05f5ffd5 367 break;
Dbee16 20:823f05f5ffd5 368 case 3:
Dbee16 20:823f05f5ffd5 369 // blue.printf("0011\n");
Dbee16 20:823f05f5ffd5 370 set_direction(0x11, 0.2,0.3);
Dbee16 20:823f05f5ffd5 371 break;
Dbee16 20:823f05f5ffd5 372 //********************************END OF DOUBLE BIT ALIGNMENT********************************************
Dbee16 20:823f05f5ffd5 373
Dbee16 20:823f05f5ffd5 374
Dbee16 20:823f05f5ffd5 375
Dbee16 20:823f05f5ffd5 376 default:
Dbee16 20:823f05f5ffd5 377 //blue.printf("%i\n", val);
Dbee16 20:823f05f5ffd5 378 set_direction(0x00, 0,0);
Dbee16 20:823f05f5ffd5 379 }
Dbee16 20:823f05f5ffd5 380 */
Dbee16 18:6e8eae661cf3 381 void sensor_read()
cmcmaster 15:3f6626f68836 382 {
Dbee16 18:6e8eae661cf3 383
cmcmaster 15:3f6626f68836 384 int x = 0;
Dbee16 18:6e8eae661cf3 385 int sens_r[4] = {0};
Dbee16 18:6e8eae661cf3 386 int sens_l[4] = {0};
Dbee16 20:823f05f5ffd5 387 val_r =0;
Dbee16 20:823f05f5ffd5 388 val_l=0;
Dbee16 18:6e8eae661cf3 389 for (x = 0; x < 8; x++) {
cmcmaster 15:3f6626f68836 390 switch(x) {
Dbee16 18:6e8eae661cf3 391 //0 is the robot's rightest sensor and 7 is the robot's leftmost sensor
Dbee16 18:6e8eae661cf3 392 //Addresses are inputs to multiplexer, can ignore them mainly
cmcmaster 15:3f6626f68836 393 case 0:
cmcmaster 15:3f6626f68836 394 sensor = 0x3;
cmcmaster 15:3f6626f68836 395 break;
cmcmaster 15:3f6626f68836 396 case 1:
cmcmaster 15:3f6626f68836 397 sensor = 0x0;
cmcmaster 15:3f6626f68836 398 break;
cmcmaster 15:3f6626f68836 399 case 2:
cmcmaster 15:3f6626f68836 400 sensor = 0x1;
cmcmaster 15:3f6626f68836 401 break;
cmcmaster 15:3f6626f68836 402 case 3:
cmcmaster 15:3f6626f68836 403 sensor = 0x2;
cmcmaster 15:3f6626f68836 404 break;
cmcmaster 15:3f6626f68836 405 case 4:
cmcmaster 15:3f6626f68836 406 sensor = 0x5;
cmcmaster 15:3f6626f68836 407 break;
cmcmaster 15:3f6626f68836 408 case 5:
cmcmaster 15:3f6626f68836 409 sensor = 0x7;
cmcmaster 15:3f6626f68836 410 break;
cmcmaster 15:3f6626f68836 411 case 6:
cmcmaster 15:3f6626f68836 412 sensor = 0x6;
cmcmaster 15:3f6626f68836 413 break;
cmcmaster 15:3f6626f68836 414 case 7:
cmcmaster 15:3f6626f68836 415 sensor = 0x4;
cmcmaster 15:3f6626f68836 416 break;
Dbee16 9:92895704e1a4 417 }
cmcmaster 15:3f6626f68836 418
Dbee16 18:6e8eae661cf3 419 if(x<4)sens_r[x] = input;
Dbee16 18:6e8eae661cf3 420 else sens_l[x%4] = input;//array of sensor output values for left and right
cmcmaster 15:3f6626f68836 421 //blue = !blue;
cmcmaster 15:3f6626f68836 422
cmcmaster 15:3f6626f68836 423
cmcmaster 15:3f6626f68836 424
Dbee16 8:eefbd3880d28 425 }
Dbee16 18:6e8eae661cf3 426 for(x = 7; x >= 0; x--) {
Dbee16 18:6e8eae661cf3 427 if(x>3) {
Dbee16 18:6e8eae661cf3 428 val_l = val_l << 1;
Dbee16 18:6e8eae661cf3 429 val_l = val_l + sens_l[x%4];
Dbee16 18:6e8eae661cf3 430 } else {
Dbee16 18:6e8eae661cf3 431 val_r = val_r << 1;
Dbee16 18:6e8eae661cf3 432 val_r = val_r + sens_r[x];
Dbee16 18:6e8eae661cf3 433 }
cmcmaster 15:3f6626f68836 434 }
Dbee16 20:823f05f5ffd5 435 //blue.printf("ValL %i \t ValR %i\n",val_l , val_r);
cmcmaster 15:3f6626f68836 436
Dbee16 8:eefbd3880d28 437 }
cmcmaster 0:59d25bb7f825 438 int main()
cmcmaster 0:59d25bb7f825 439 {
Dbee16 20:823f05f5ffd5 440 wait(0.5);
Dbee16 20:823f05f5ffd5 441 led = !led;
Dbee16 20:823f05f5ffd5 442 blue.printf("Starting Code!!!!!!\n");
Dbee16 17:54da4359134f 443 level = 0.58; //Analogout Level for black line into comparator
Dbee16 2:1feae3cb6731 444 //Set PWM frequency to 1000Hz
cmcmaster 0:59d25bb7f825 445 motor_l.period( 1.0f / (float) PWM_FREQ);
cmcmaster 0:59d25bb7f825 446 motor_r.period( 1.0f / (float) PWM_FREQ);
cmcmaster 0:59d25bb7f825 447 //Initialise direction to nothing.
cmcmaster 0:59d25bb7f825 448 motor_rf=0;
cmcmaster 0:59d25bb7f825 449 motor_rb=0;
cmcmaster 0:59d25bb7f825 450 motor_lf=0;
cmcmaster 0:59d25bb7f825 451 motor_lb=0;
Dbee16 18:6e8eae661cf3 452 //Now Global*****
Dbee16 18:6e8eae661cf3 453 //int val =0;
Dbee16 18:6e8eae661cf3 454 //int old_val=0;*********
Dbee16 18:6e8eae661cf3 455
Dbee16 18:6e8eae661cf3 456 while(1) { //PRINTS A STRING 0110 0110 FOR LEFT AND RIGHT RESPECTIVELY
Dbee16 20:823f05f5ffd5 457 led=!led;
Dbee16 18:6e8eae661cf3 458 old_val_r = val_r;
Dbee16 18:6e8eae661cf3 459 old_val_l = val_l;
Dbee16 18:6e8eae661cf3 460 sensor_read();
Dbee16 18:6e8eae661cf3 461 if((!(old_val_l == val_l)) | (!(old_val_r == val_r))) {
Dbee16 18:6e8eae661cf3 462 string binary_r = "";
Dbee16 18:6e8eae661cf3 463 string binary_l = "";
Dbee16 17:54da4359134f 464 int mask = 1;
Dbee16 17:54da4359134f 465 for(int i = 0; i < 4; i++) {
Dbee16 20:823f05f5ffd5 466 if((mask&val_r)) binary_r = "1"+binary_r; //(mask&val) >= 1
Dbee16 18:6e8eae661cf3 467 else binary_r = "0"+binary_r;
Dbee16 20:823f05f5ffd5 468 if((mask&val_l)) binary_l = "1"+binary_l; //(mask&val) >= 1
Dbee16 18:6e8eae661cf3 469 else binary_l = "0"+binary_l;
Dbee16 17:54da4359134f 470 mask = mask << 1;
Dbee16 17:54da4359134f 471 }
Dbee16 20:823f05f5ffd5 472 blue.printf("L %s\t R %s\n", binary_l, binary_r);
Dbee16 18:6e8eae661cf3 473 motor_result();
Dbee16 17:54da4359134f 474 }
Dbee16 20:823f05f5ffd5 475 wait(0.2);
Dbee16 17:54da4359134f 476 // if(!(old_val == val)) {
Dbee16 17:54da4359134f 477 // motor_result(val);
Dbee16 17:54da4359134f 478 // }
cmcmaster 15:3f6626f68836 479 }
cmcmaster 15:3f6626f68836 480 // while(1) {
cmcmaster 15:3f6626f68836 481 //Working
cmcmaster 15:3f6626f68836 482 /*
cmcmaster 15:3f6626f68836 483 led = !led;
cmcmaster 15:3f6626f68836 484 wait(4);
cmcmaster 15:3f6626f68836 485 blue.printf("Forward, full\n");
cmcmaster 15:3f6626f68836 486 set_direction(0x11, 1.0,1.0);//forward full speed
cmcmaster 15:3f6626f68836 487 wait(4);
cmcmaster 15:3f6626f68836 488 blue.printf("Forward, left half\n");
cmcmaster 15:3f6626f68836 489 led = !led;
cmcmaster 15:3f6626f68836 490 set_direction(0x11, 0.5 ,0); //forward left half
cmcmaster 15:3f6626f68836 491 wait(4);
cmcmaster 15:3f6626f68836 492 blue.printf("Forward, right half\n");
cmcmaster 15:3f6626f68836 493 led = !led;
cmcmaster 15:3f6626f68836 494 set_direction(0x11, 0 ,0.5); //forward right half
cmcmaster 15:3f6626f68836 495 wait(4);
cmcmaster 15:3f6626f68836 496 led = !led;
cmcmaster 15:3f6626f68836 497 blue.printf("stop\n");
cmcmaster 15:3f6626f68836 498 set_direction(0x00, 0 ,0); //stop
cmcmaster 15:3f6626f68836 499 wait(4);
cmcmaster 15:3f6626f68836 500 led = !led;
cmcmaster 15:3f6626f68836 501 blue.printf("Back, full\n");
cmcmaster 15:3f6626f68836 502 set_direction(0x00, 1.0 ,1.0); //backward full speed
cmcmaster 15:3f6626f68836 503 wait(4);
cmcmaster 15:3f6626f68836 504 led = !led;
cmcmaster 15:3f6626f68836 505 blue.printf("Back, left\n");
cmcmaster 15:3f6626f68836 506 set_direction(0x00, 0.5 ,0);
cmcmaster 15:3f6626f68836 507 wait(4);
cmcmaster 15:3f6626f68836 508 blue.printf("Back, right\n");
cmcmaster 15:3f6626f68836 509 led = !led;
cmcmaster 15:3f6626f68836 510 set_direction(0x00, 0 ,0.5);
Dbee16 11:f07662c64e26 511
cmcmaster 15:3f6626f68836 512 */
Dbee16 11:f07662c64e26 513
cmcmaster 15:3f6626f68836 514 //Sensor Code + Motor Test
cmcmaster 15:3f6626f68836 515 //int values = 0;
cmcmaster 15:3f6626f68836 516 // values = sensor_read();
cmcmaster 15:3f6626f68836 517 // change_direction(values);
Dbee16 9:92895704e1a4 518
cmcmaster 15:3f6626f68836 519 //}
Dbee16 9:92895704e1a4 520 }
Dbee16 8:eefbd3880d28 521
Dbee16 8:eefbd3880d28 522
Dbee16 8:eefbd3880d28 523
Dbee16 8:eefbd3880d28 524
Dbee16 8:eefbd3880d28 525