Team Ascent / Mbed 2 deprecated TDPCode1

Dependencies:   mbed

Committer:
Dbee16
Date:
Tue Mar 24 15:55:31 2015 +0000
Revision:
20:823f05f5ffd5
Parent:
18:6e8eae661cf3
Child:
21:bef848ac1894
Line sensor new code; needs to fucking test right fucking now

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 18:6e8eae661cf3 42 int forwards = 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 20:823f05f5ffd5 109 forwards = 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 20:823f05f5ffd5 115 forwards =0;
Dbee16 20:823f05f5ffd5 116 }
Dbee16 20:823f05f5ffd5 117 } else if(val_l == 7) {
Dbee16 20:823f05f5ffd5 118 l_turn_f =1;
Dbee16 20:823f05f5ffd5 119 if(!forwards) {
Dbee16 20:823f05f5ffd5 120 forwards =1;
Dbee16 20:823f05f5ffd5 121 set_direction(0x11, 0.3,0.3);
Dbee16 20:823f05f5ffd5 122 } else if(!forwards) {
Dbee16 20:823f05f5ffd5 123 forwards =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 20:823f05f5ffd5 145 forwards =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 20:823f05f5ffd5 171 set_direction(0x10, 0.3 0.3);
Dbee16 20:823f05f5ffd5 172 forward = 0;
Dbee16 20:823f05f5ffd5 173 spin = 1;
Dbee16 20:823f05f5ffd5 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 20:823f05f5ffd5 178 }
Dbee16 20:823f05f5ffd5 179 else {
Dbee16 20:823f05f5ffd5 180 r_180_f =1;
Dbee16 20:823f05f5ffd5 181 }
Dbee16 9:92895704e1a4 182 break;
Dbee16 20:823f05f5ffd5 183
Dbee16 20:823f05f5ffd5 184
cmcmaster 15:3f6626f68836 185 case 2:
Dbee16 17:54da4359134f 186 // blue.printf("0010\n");
Dbee16 20:823f05f5ffd5 187 r_normal();
cmcmaster 15:3f6626f68836 188 break;
cmcmaster 15:3f6626f68836 189 case 4:
Dbee16 17:54da4359134f 190 // blue.printf("0100\n");
Dbee16 20:823f05f5ffd5 191 r_normal();
Dbee16 9:92895704e1a4 192 break;
Dbee16 20:823f05f5ffd5 193
Dbee16 20:823f05f5ffd5 194 //***************************IF THERE IS A SINGLE BIT ONLY; WE ARE very MISALIGNED*********************************
Dbee16 20:823f05f5ffd5 195 case 1:
Dbee16 20:823f05f5ffd5 196 // blue.printf("0001\n");
Dbee16 20:823f05f5ffd5 197 set_direction(0x11, 0,0);
Dbee16 20:823f05f5ffd5 198 forward=0;
Dbee16 20:823f05f5ffd5 199 break;
Dbee16 20:823f05f5ffd5 200
Dbee16 18:6e8eae661cf3 201 case 8:
Dbee16 18:6e8eae661cf3 202 //blue.printf("1000\n");
Dbee16 20:823f05f5ffd5 203 set_direction(0x11, 0,0);
Dbee16 20:823f05f5ffd5 204 forward = 0;
Dbee16 17:54da4359134f 205 break;
Dbee16 18:6e8eae661cf3 206 //******************************END OF SINGLE BIT ALIGNMENT********************************************
Dbee16 18:6e8eae661cf3 207
Dbee16 18:6e8eae661cf3 208 //*************IF TWO BITS ARE ON, AND NOT IN THE MIDDLE, WE'RE PROBABLY VERY MISALIGNED*******************
Dbee16 17:54da4359134f 209 case 12:
Dbee16 17:54da4359134f 210 // blue.printf("1100\n");
Dbee16 17:54da4359134f 211 set_direction(0x11, 0.2,0.3);
Dbee16 20:823f05f5ffd5 212 forward = 0;
Dbee16 17:54da4359134f 213 break;
Dbee16 17:54da4359134f 214 case 3:
Dbee16 17:54da4359134f 215 // blue.printf("0011\n");
Dbee16 17:54da4359134f 216 set_direction(0x11, 0.3,0.2);
Dbee16 20:823f05f5ffd5 217 forward=0;
cmcmaster 15:3f6626f68836 218 break;
Dbee16 18:6e8eae661cf3 219 //********************************END OF DOUBLE BIT ALIGNMENT********************************************
Dbee16 18:6e8eae661cf3 220
Dbee16 20:823f05f5ffd5 221 case 15:
Dbee16 20:823f05f5ffd5 222 set_direction(0x11, 0,0);
Dbee16 20:823f05f5ffd5 223 forward =0;
Dbee16 20:823f05f5ffd5 224 break;
Dbee16 18:6e8eae661cf3 225
Dbee16 18:6e8eae661cf3 226 default:
Dbee16 18:6e8eae661cf3 227 //blue.printf("%i\n", val);
Dbee16 18:6e8eae661cf3 228 set_direction(0x00, 0,0);
Dbee16 20:823f05f5ffd5 229 }
Dbee16 18:6e8eae661cf3 230 }
Dbee16 20:823f05f5ffd5 231 }
Dbee16 18:6e8eae661cf3 232
Dbee16 20:823f05f5ffd5 233 /*
Dbee16 20:823f05f5ffd5 234 //This needed to be completely changed. We need sequential actions, a case statement is too basic (unforetuneately)
Dbee16 20:823f05f5ffd5 235 led= 0;
Dbee16 20:823f05f5ffd5 236 switch(val_r) {
Dbee16 20:823f05f5ffd5 237 case 4: //Normal mode
Dbee16 20:823f05f5ffd5 238 if (!forwards) {
Dbee16 20:823f05f5ffd5 239 blue.printf("i've got %i on the right\n", val_r);
Dbee16 20:823f05f5ffd5 240 set_direction(0x11, 0.3,0.3);
Dbee16 20:823f05f5ffd5 241 forwards =1;
Dbee16 20:823f05f5ffd5 242 change =1;
Dbee16 20:823f05f5ffd5 243 break;
Dbee16 20:823f05f5ffd5 244 }
Dbee16 20:823f05f5ffd5 245 case 12:
Dbee16 20:823f05f5ffd5 246 blue.printf("i've got %i on the right\n", val_r);
Dbee16 18:6e8eae661cf3 247
Dbee16 20:823f05f5ffd5 248 set_direction(0x11, 0.3, 0.2);
Dbee16 20:823f05f5ffd5 249 forwards =0;
Dbee16 20:823f05f5ffd5 250 break;
Dbee16 20:823f05f5ffd5 251 change =1;
Dbee16 18:6e8eae661cf3 252
Dbee16 20:823f05f5ffd5 253 case 8:
Dbee16 20:823f05f5ffd5 254 blue.printf("i've got %i on the right\n", val_r);
Dbee16 20:823f05f5ffd5 255
Dbee16 20:823f05f5ffd5 256 set_direction(0x11, 0.2, 0.3);
Dbee16 20:823f05f5ffd5 257 forwards =0;
Dbee16 20:823f05f5ffd5 258 change =1;
Dbee16 18:6e8eae661cf3 259
Dbee16 20:823f05f5ffd5 260 break;
Dbee16 20:823f05f5ffd5 261 default:
Dbee16 20:823f05f5ffd5 262 if (!forwards) {
Dbee16 20:823f05f5ffd5 263 blue.printf("i've got %i on the right\n", val_r);
Dbee16 20:823f05f5ffd5 264
Dbee16 20:823f05f5ffd5 265 set_direction(0x11, 0.3,0.3);
Dbee16 20:823f05f5ffd5 266 forwards =1;
Dbee16 20:823f05f5ffd5 267 break;
Dbee16 20:823f05f5ffd5 268 }
Dbee16 20:823f05f5ffd5 269
Dbee16 20:823f05f5ffd5 270 }
Dbee16 20:823f05f5ffd5 271 if(!change) {
Dbee16 20:823f05f5ffd5 272 switch(val_l) {
Dbee16 20:823f05f5ffd5 273 case 2:
Dbee16 20:823f05f5ffd5 274 if (!forwards) {
Dbee16 20:823f05f5ffd5 275 blue.printf("i've got %i on the left\n", val_l);
Dbee16 18:6e8eae661cf3 276
Dbee16 20:823f05f5ffd5 277 set_direction(0x11, 0.3,0.3);
Dbee16 20:823f05f5ffd5 278 forwards =1;
Dbee16 20:823f05f5ffd5 279 break;
Dbee16 20:823f05f5ffd5 280 }
Dbee16 20:823f05f5ffd5 281 case 3:
Dbee16 20:823f05f5ffd5 282 blue.printf("i've got %i on the left\n", val_l);
Dbee16 18:6e8eae661cf3 283
Dbee16 20:823f05f5ffd5 284 set_direction(0x11, 0.3, 0.2);
Dbee16 20:823f05f5ffd5 285 forwards =0;
Dbee16 20:823f05f5ffd5 286 break;
Dbee16 20:823f05f5ffd5 287 case 1:
Dbee16 20:823f05f5ffd5 288 blue.printf("i've got %i on the left\n", val_l);
Dbee16 20:823f05f5ffd5 289
Dbee16 20:823f05f5ffd5 290 set_direction(0x11, 0.3, 0.2);
Dbee16 20:823f05f5ffd5 291 forwards =0;
Dbee16 20:823f05f5ffd5 292 break;
Dbee16 18:6e8eae661cf3 293
Dbee16 18:6e8eae661cf3 294
Dbee16 18:6e8eae661cf3 295
Dbee16 20:823f05f5ffd5 296 default:
Dbee16 20:823f05f5ffd5 297 blue.printf("i've got %i on the left\n", val_l);
Dbee16 20:823f05f5ffd5 298 if (!forwards) {
Dbee16 20:823f05f5ffd5 299 // blue.printf("i've got %i on the left\n", val_l);
Dbee16 20:823f05f5ffd5 300
Dbee16 20:823f05f5ffd5 301 set_direction(0x11, 0.3,0.3);
Dbee16 20:823f05f5ffd5 302 forwards =1;
Dbee16 20:823f05f5ffd5 303 break;
Dbee16 20:823f05f5ffd5 304 }
Dbee16 20:823f05f5ffd5 305 }
Dbee16 20:823f05f5ffd5 306 }*/
Dbee16 20:823f05f5ffd5 307 //********************************************************START******************************************************
Dbee16 20:823f05f5ffd5 308
Dbee16 20:823f05f5ffd5 309
Dbee16 20:823f05f5ffd5 310 //***************************************LEFT SHOULD BE PRETTY MUCH MIRRORED***********************************************************
Dbee16 20:823f05f5ffd5 311 /* switch(val_l) {
Dbee16 20:823f05f5ffd5 312 case 7: //Normal Mode
Dbee16 20:823f05f5ffd5 313 // blue.printf("0110\n");
Dbee16 20:823f05f5ffd5 314 set_direction(0x11, 0.3,0.3);
Dbee16 20:823f05f5ffd5 315 break;
Dbee16 20:823f05f5ffd5 316 // case 7: // 0111 Either misaligned or getting ready for a turn
Dbee16 20:823f05f5ffd5 317 // l_turn_f = 1;
Dbee16 20:823f05f5ffd5 318 //No need to change direction, if misaligned, it will soon go to a double line alignment protocol
Dbee16 20:823f05f5ffd5 319 //if the next state is 0000; then we were right and there is a left turn.
Dbee16 20:823f05f5ffd5 320 // break;
Dbee16 20:823f05f5ffd5 321 case 0: //lost or turn
Dbee16 20:823f05f5ffd5 322 // blue.printf("0000\n");
Dbee16 20:823f05f5ffd5 323 if (r_turn_f & l_turn_f) { //if we're expecting a junction
Dbee16 20:823f05f5ffd5 324 set_direction(0x10, 0.3,0.3); //spin right because there's only one junction
Dbee16 20:823f05f5ffd5 325 //no stop conditions yet
Dbee16 20:823f05f5ffd5 326
Dbee16 20:823f05f5ffd5 327 } else if (l_turn_f) { //if we're expecting a left turn
Dbee16 20:823f05f5ffd5 328 set_direction(0x01, 0.3,0.3); //spin left
Dbee16 20:823f05f5ffd5 329 //no stop conditions yet
Dbee16 20:823f05f5ffd5 330
Dbee16 20:823f05f5ffd5 331 } else if(val_r == 6) { //0000 0110 could mean a left 180 turn
Dbee16 20:823f05f5ffd5 332 //keep going forward, no need to change dir
Dbee16 20:823f05f5ffd5 333 l_180_f = 1; //raise 180 flag in prep for next time
cmcmaster 15:3f6626f68836 334
Dbee16 20:823f05f5ffd5 335 } else if(l_180_f) {
Dbee16 20:823f05f5ffd5 336 //code for 180???
Dbee16 20:823f05f5ffd5 337 blue.printf("I want to 180 left, but i don't know how yet");
Dbee16 20:823f05f5ffd5 338 set_direction(0x00, 0,0);
Dbee16 20:823f05f5ffd5 339
Dbee16 20:823f05f5ffd5 340 } else { //probably lost
Dbee16 20:823f05f5ffd5 341 set_direction(0x00, 0,0);
Dbee16 20:823f05f5ffd5 342 }
Dbee16 20:823f05f5ffd5 343 break;
Dbee16 20:823f05f5ffd5 344
Dbee16 20:823f05f5ffd5 345 //***************************IF THERE IS A SINGLE BIT ONLY; WE ARE MISALIGNED*********************************
Dbee16 20:823f05f5ffd5 346 case 1:
Dbee16 20:823f05f5ffd5 347 // blue.printf("0001\n");
Dbee16 20:823f05f5ffd5 348 set_direction(0x11, 0.1,0.3);
Dbee16 20:823f05f5ffd5 349 break;
Dbee16 20:823f05f5ffd5 350 case 2:
Dbee16 20:823f05f5ffd5 351 // blue.printf("0010\n");
Dbee16 20:823f05f5ffd5 352 set_direction(0x11, 0.2,0.3);
Dbee16 20:823f05f5ffd5 353 break;
Dbee16 20:823f05f5ffd5 354 case 4:
Dbee16 20:823f05f5ffd5 355 // blue.printf("0100\n");
Dbee16 20:823f05f5ffd5 356 set_direction(0x11, 0.3,0.2);
Dbee16 20:823f05f5ffd5 357 break;
Dbee16 20:823f05f5ffd5 358 case 8:
Dbee16 20:823f05f5ffd5 359 //blue.printf("1000\n");
Dbee16 20:823f05f5ffd5 360 set_direction(0x11, 0.3,0.1);
Dbee16 20:823f05f5ffd5 361 break;
Dbee16 20:823f05f5ffd5 362 //******************************END OF SINGLE BIT ALIGNMENT********************************************
Dbee16 20:823f05f5ffd5 363
Dbee16 20:823f05f5ffd5 364 //*************IF TWO BITS ARE ON, AND NOT IN THE MIDDLE, WE'RE PROBABLY VERY MISALIGNED*******************
Dbee16 20:823f05f5ffd5 365 case 12:
Dbee16 20:823f05f5ffd5 366 // blue.printf("1100\n");
Dbee16 20:823f05f5ffd5 367 set_direction(0x11, 0.3,0.2);
Dbee16 20:823f05f5ffd5 368 break;
Dbee16 20:823f05f5ffd5 369 case 3:
Dbee16 20:823f05f5ffd5 370 // blue.printf("0011\n");
Dbee16 20:823f05f5ffd5 371 set_direction(0x11, 0.2,0.3);
Dbee16 20:823f05f5ffd5 372 break;
Dbee16 20:823f05f5ffd5 373 //********************************END OF DOUBLE BIT ALIGNMENT********************************************
Dbee16 20:823f05f5ffd5 374
Dbee16 20:823f05f5ffd5 375
Dbee16 20:823f05f5ffd5 376
Dbee16 20:823f05f5ffd5 377 default:
Dbee16 20:823f05f5ffd5 378 //blue.printf("%i\n", val);
Dbee16 20:823f05f5ffd5 379 set_direction(0x00, 0,0);
Dbee16 20:823f05f5ffd5 380 }
Dbee16 20:823f05f5ffd5 381 */
Dbee16 18:6e8eae661cf3 382 void sensor_read()
cmcmaster 15:3f6626f68836 383 {
Dbee16 18:6e8eae661cf3 384
cmcmaster 15:3f6626f68836 385 int x = 0;
Dbee16 18:6e8eae661cf3 386 int sens_r[4] = {0};
Dbee16 18:6e8eae661cf3 387 int sens_l[4] = {0};
Dbee16 20:823f05f5ffd5 388 val_r =0;
Dbee16 20:823f05f5ffd5 389 val_l=0;
Dbee16 18:6e8eae661cf3 390 for (x = 0; x < 8; x++) {
cmcmaster 15:3f6626f68836 391 switch(x) {
Dbee16 18:6e8eae661cf3 392 //0 is the robot's rightest sensor and 7 is the robot's leftmost sensor
Dbee16 18:6e8eae661cf3 393 //Addresses are inputs to multiplexer, can ignore them mainly
cmcmaster 15:3f6626f68836 394 case 0:
cmcmaster 15:3f6626f68836 395 sensor = 0x3;
cmcmaster 15:3f6626f68836 396 break;
cmcmaster 15:3f6626f68836 397 case 1:
cmcmaster 15:3f6626f68836 398 sensor = 0x0;
cmcmaster 15:3f6626f68836 399 break;
cmcmaster 15:3f6626f68836 400 case 2:
cmcmaster 15:3f6626f68836 401 sensor = 0x1;
cmcmaster 15:3f6626f68836 402 break;
cmcmaster 15:3f6626f68836 403 case 3:
cmcmaster 15:3f6626f68836 404 sensor = 0x2;
cmcmaster 15:3f6626f68836 405 break;
cmcmaster 15:3f6626f68836 406 case 4:
cmcmaster 15:3f6626f68836 407 sensor = 0x5;
cmcmaster 15:3f6626f68836 408 break;
cmcmaster 15:3f6626f68836 409 case 5:
cmcmaster 15:3f6626f68836 410 sensor = 0x7;
cmcmaster 15:3f6626f68836 411 break;
cmcmaster 15:3f6626f68836 412 case 6:
cmcmaster 15:3f6626f68836 413 sensor = 0x6;
cmcmaster 15:3f6626f68836 414 break;
cmcmaster 15:3f6626f68836 415 case 7:
cmcmaster 15:3f6626f68836 416 sensor = 0x4;
cmcmaster 15:3f6626f68836 417 break;
Dbee16 9:92895704e1a4 418 }
cmcmaster 15:3f6626f68836 419
Dbee16 18:6e8eae661cf3 420 if(x<4)sens_r[x] = input;
Dbee16 18:6e8eae661cf3 421 else sens_l[x%4] = input;//array of sensor output values for left and right
cmcmaster 15:3f6626f68836 422 //blue = !blue;
cmcmaster 15:3f6626f68836 423
cmcmaster 15:3f6626f68836 424
cmcmaster 15:3f6626f68836 425
Dbee16 8:eefbd3880d28 426 }
Dbee16 18:6e8eae661cf3 427 for(x = 7; x >= 0; x--) {
Dbee16 18:6e8eae661cf3 428 if(x>3) {
Dbee16 18:6e8eae661cf3 429 val_l = val_l << 1;
Dbee16 18:6e8eae661cf3 430 val_l = val_l + sens_l[x%4];
Dbee16 18:6e8eae661cf3 431 } else {
Dbee16 18:6e8eae661cf3 432 val_r = val_r << 1;
Dbee16 18:6e8eae661cf3 433 val_r = val_r + sens_r[x];
Dbee16 18:6e8eae661cf3 434 }
cmcmaster 15:3f6626f68836 435 }
Dbee16 20:823f05f5ffd5 436 //blue.printf("ValL %i \t ValR %i\n",val_l , val_r);
cmcmaster 15:3f6626f68836 437
Dbee16 8:eefbd3880d28 438 }
cmcmaster 0:59d25bb7f825 439 int main()
cmcmaster 0:59d25bb7f825 440 {
Dbee16 20:823f05f5ffd5 441 wait(0.5);
Dbee16 20:823f05f5ffd5 442 led = !led;
Dbee16 20:823f05f5ffd5 443 blue.printf("Starting Code!!!!!!\n");
Dbee16 17:54da4359134f 444 level = 0.58; //Analogout Level for black line into comparator
Dbee16 2:1feae3cb6731 445 //Set PWM frequency to 1000Hz
cmcmaster 0:59d25bb7f825 446 motor_l.period( 1.0f / (float) PWM_FREQ);
cmcmaster 0:59d25bb7f825 447 motor_r.period( 1.0f / (float) PWM_FREQ);
cmcmaster 0:59d25bb7f825 448 //Initialise direction to nothing.
cmcmaster 0:59d25bb7f825 449 motor_rf=0;
cmcmaster 0:59d25bb7f825 450 motor_rb=0;
cmcmaster 0:59d25bb7f825 451 motor_lf=0;
cmcmaster 0:59d25bb7f825 452 motor_lb=0;
Dbee16 18:6e8eae661cf3 453 //Now Global*****
Dbee16 18:6e8eae661cf3 454 //int val =0;
Dbee16 18:6e8eae661cf3 455 //int old_val=0;*********
Dbee16 18:6e8eae661cf3 456
Dbee16 18:6e8eae661cf3 457 while(1) { //PRINTS A STRING 0110 0110 FOR LEFT AND RIGHT RESPECTIVELY
Dbee16 20:823f05f5ffd5 458 led=!led;
Dbee16 18:6e8eae661cf3 459 old_val_r = val_r;
Dbee16 18:6e8eae661cf3 460 old_val_l = val_l;
Dbee16 18:6e8eae661cf3 461 sensor_read();
Dbee16 18:6e8eae661cf3 462 if((!(old_val_l == val_l)) | (!(old_val_r == val_r))) {
Dbee16 18:6e8eae661cf3 463 string binary_r = "";
Dbee16 18:6e8eae661cf3 464 string binary_l = "";
Dbee16 17:54da4359134f 465 int mask = 1;
Dbee16 17:54da4359134f 466 for(int i = 0; i < 4; i++) {
Dbee16 20:823f05f5ffd5 467 if((mask&val_r)) binary_r = "1"+binary_r; //(mask&val) >= 1
Dbee16 18:6e8eae661cf3 468 else binary_r = "0"+binary_r;
Dbee16 20:823f05f5ffd5 469 if((mask&val_l)) binary_l = "1"+binary_l; //(mask&val) >= 1
Dbee16 18:6e8eae661cf3 470 else binary_l = "0"+binary_l;
Dbee16 17:54da4359134f 471 mask = mask << 1;
Dbee16 17:54da4359134f 472 }
Dbee16 20:823f05f5ffd5 473 blue.printf("L %s\t R %s\n", binary_l, binary_r);
Dbee16 18:6e8eae661cf3 474 motor_result();
Dbee16 17:54da4359134f 475 }
Dbee16 20:823f05f5ffd5 476 wait(0.2);
Dbee16 17:54da4359134f 477 // if(!(old_val == val)) {
Dbee16 17:54da4359134f 478 // motor_result(val);
Dbee16 17:54da4359134f 479 // }
cmcmaster 15:3f6626f68836 480 }
cmcmaster 15:3f6626f68836 481 // while(1) {
cmcmaster 15:3f6626f68836 482 //Working
cmcmaster 15:3f6626f68836 483 /*
cmcmaster 15:3f6626f68836 484 led = !led;
cmcmaster 15:3f6626f68836 485 wait(4);
cmcmaster 15:3f6626f68836 486 blue.printf("Forward, full\n");
cmcmaster 15:3f6626f68836 487 set_direction(0x11, 1.0,1.0);//forward full speed
cmcmaster 15:3f6626f68836 488 wait(4);
cmcmaster 15:3f6626f68836 489 blue.printf("Forward, left half\n");
cmcmaster 15:3f6626f68836 490 led = !led;
cmcmaster 15:3f6626f68836 491 set_direction(0x11, 0.5 ,0); //forward left half
cmcmaster 15:3f6626f68836 492 wait(4);
cmcmaster 15:3f6626f68836 493 blue.printf("Forward, right half\n");
cmcmaster 15:3f6626f68836 494 led = !led;
cmcmaster 15:3f6626f68836 495 set_direction(0x11, 0 ,0.5); //forward right half
cmcmaster 15:3f6626f68836 496 wait(4);
cmcmaster 15:3f6626f68836 497 led = !led;
cmcmaster 15:3f6626f68836 498 blue.printf("stop\n");
cmcmaster 15:3f6626f68836 499 set_direction(0x00, 0 ,0); //stop
cmcmaster 15:3f6626f68836 500 wait(4);
cmcmaster 15:3f6626f68836 501 led = !led;
cmcmaster 15:3f6626f68836 502 blue.printf("Back, full\n");
cmcmaster 15:3f6626f68836 503 set_direction(0x00, 1.0 ,1.0); //backward full speed
cmcmaster 15:3f6626f68836 504 wait(4);
cmcmaster 15:3f6626f68836 505 led = !led;
cmcmaster 15:3f6626f68836 506 blue.printf("Back, left\n");
cmcmaster 15:3f6626f68836 507 set_direction(0x00, 0.5 ,0);
cmcmaster 15:3f6626f68836 508 wait(4);
cmcmaster 15:3f6626f68836 509 blue.printf("Back, right\n");
cmcmaster 15:3f6626f68836 510 led = !led;
cmcmaster 15:3f6626f68836 511 set_direction(0x00, 0 ,0.5);
Dbee16 11:f07662c64e26 512
cmcmaster 15:3f6626f68836 513 */
Dbee16 11:f07662c64e26 514
cmcmaster 15:3f6626f68836 515 //Sensor Code + Motor Test
cmcmaster 15:3f6626f68836 516 //int values = 0;
cmcmaster 15:3f6626f68836 517 // values = sensor_read();
cmcmaster 15:3f6626f68836 518 // change_direction(values);
Dbee16 9:92895704e1a4 519
cmcmaster 15:3f6626f68836 520 //}
Dbee16 9:92895704e1a4 521 }
Dbee16 8:eefbd3880d28 522
Dbee16 8:eefbd3880d28 523
Dbee16 8:eefbd3880d28 524
Dbee16 8:eefbd3880d28 525
Dbee16 8:eefbd3880d28 526