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@20:823f05f5ffd5, 2015-03-24 (annotated)
- 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?
User | Revision | Line number | New 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 |