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

Committer:
Dbee16
Date:
Wed Mar 11 22:01:14 2015 +0000
Revision:
6:dd0a91c2994f
Parent:
5:22d7fee2e26e 
Added questions I have and a commented fix

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cmcmaster 0:59d25bb7f825 1 #include "mbed.h"
cmcmaster 0:59d25bb7f825 2
cmcmaster 5:22d7fee2e26e 3 Serial blue(PTC4,PTC3);
cmcmaster 5:22d7fee2e26e 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
cmcmaster 5:22d7fee2e26e 11
cmcmaster 5:22d7fee2e26e 12 DigitalIn input(PTC1); //input from sensor array
cmcmaster 5:22d7fee2e26e 13 DigitalOut Line_right(LED_GREEN);//no line detected
cmcmaster 5:22d7fee2e26e 14 //DigitalOut blue(LED_BLUE);
cmcmaster 5:22d7fee2e26e 15
cmcmaster 5:22d7fee2e26e 16 BusOut sensor(PTD7,PTE1,PTE0);//multiplex sensors
cmcmaster 5:22d7fee2e26e 17 AnalogOut level(PTE30);//set comparator level needs to be tuned for each sensor (could create program)
cmcmaster 5:22d7fee2e26e 18
cmcmaster 5:22d7fee2e26e 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
cmcmaster 5:22d7fee2e26e 30 void sensor_select(int x)//converts int to hex for display
cmcmaster 5:22d7fee2e26e 31 {
cmcmaster 5:22d7fee2e26e 32 switch(x) {
cmcmaster 5:22d7fee2e26e 33 case 0:
cmcmaster 5:22d7fee2e26e 34 sensor = 0x2;
cmcmaster 5:22d7fee2e26e 35 break;
cmcmaster 5:22d7fee2e26e 36 case 1:
cmcmaster 5:22d7fee2e26e 37 sensor = 0x1;
cmcmaster 5:22d7fee2e26e 38 break;
cmcmaster 5:22d7fee2e26e 39 case 2:
cmcmaster 5:22d7fee2e26e 40 sensor = 0x0;
cmcmaster 5:22d7fee2e26e 41 break;
cmcmaster 5:22d7fee2e26e 42 case 3:
cmcmaster 5:22d7fee2e26e 43 sensor = 0x3;
cmcmaster 5:22d7fee2e26e 44 break;
cmcmaster 5:22d7fee2e26e 45 case 4:
cmcmaster 5:22d7fee2e26e 46 sensor = 0x5;
cmcmaster 5:22d7fee2e26e 47 break;
cmcmaster 5:22d7fee2e26e 48 case 5:
cmcmaster 5:22d7fee2e26e 49 sensor = 0x7;
cmcmaster 5:22d7fee2e26e 50 break;
cmcmaster 5:22d7fee2e26e 51 case 6:
cmcmaster 5:22d7fee2e26e 52 sensor = 0x6;
cmcmaster 5:22d7fee2e26e 53 break;
cmcmaster 5:22d7fee2e26e 54 case 7:
cmcmaster 5:22d7fee2e26e 55 sensor = 0x4;
cmcmaster 5:22d7fee2e26e 56 break;
cmcmaster 5:22d7fee2e26e 57 }
cmcmaster 5:22d7fee2e26e 58
cmcmaster 5:22d7fee2e26e 59 }
cmcmaster 5:22d7fee2e26e 60 int sensor_read()
cmcmaster 5:22d7fee2e26e 61 {
cmcmaster 5:22d7fee2e26e 62 int val=0;
cmcmaster 5:22d7fee2e26e 63 level = 0.3;
cmcmaster 5:22d7fee2e26e 64 int x = 0;
cmcmaster 5:22d7fee2e26e 65
cmcmaster 5:22d7fee2e26e 66 //static int values[8];
Dbee16 6:dd0a91c2994f 67 while( x <= 3 ) { //why 3? Don't you want to select through 8 sensors therefore x<=7?
cmcmaster 5:22d7fee2e26e 68 sensor_select(x);
cmcmaster 5:22d7fee2e26e 69 //blue = !blue;
cmcmaster 5:22d7fee2e26e 70 wait(0.1);
cmcmaster 5:22d7fee2e26e 71
cmcmaster 5:22d7fee2e26e 72 if (input == 1) {
Dbee16 6:dd0a91c2994f 73 val=val+2^x; //could you comment on what this is for?
cmcmaster 5:22d7fee2e26e 74 } else {
cmcmaster 5:22d7fee2e26e 75 val=val;//+2^x;
cmcmaster 5:22d7fee2e26e 76 }
cmcmaster 5:22d7fee2e26e 77 x=x+1;
cmcmaster 5:22d7fee2e26e 78
cmcmaster 5:22d7fee2e26e 79 }
cmcmaster 5:22d7fee2e26e 80 blue.printf("%i\n",val);
cmcmaster 5:22d7fee2e26e 81 return val;
cmcmaster 5:22d7fee2e26e 82 }
cmcmaster 5:22d7fee2e26e 83
cmcmaster 5:22d7fee2e26e 84
Dbee16 2:1feae3cb6731 85 void set_direction( int direction, float speed, float angle)
cmcmaster 0:59d25bb7f825 86 {
cmcmaster 5:22d7fee2e26e 87
Dbee16 2:1feae3cb6731 88
Dbee16 2:1feae3cb6731 89 float r_result = (speed + angle); //where angle can be postive and negative and will TURN the robot
Dbee16 2:1feae3cb6731 90 float l_result = (speed - angle); //if turning left, you'd make the angle postive (right motor goes faster left motor goes slwoer)
Dbee16 2:1feae3cb6731 91 switch( direction ) {
Dbee16 2:1feae3cb6731 92 case 0x11: { //forward
Dbee16 2:1feae3cb6731 93 motor_r.write( r_result < 0 ? 0 : r_result > 1 ? 1 : r_result);
Dbee16 2:1feae3cb6731 94 //Look up ternary statements -- {condition ? value_if_true : value_if_false} This makes 0<result<1
Dbee16 2:1feae3cb6731 95 motor_l.write( l_result < 0 ? 0 : l_result > 1 ? 1 : l_result);
Dbee16 2:1feae3cb6731 96
cmcmaster 0:59d25bb7f825 97 motor_rf=1;
cmcmaster 0:59d25bb7f825 98 motor_rb=0;
cmcmaster 0:59d25bb7f825 99 motor_lf=1;
cmcmaster 0:59d25bb7f825 100 motor_lb=0;
cmcmaster 0:59d25bb7f825 101 }
Dbee16 2:1feae3cb6731 102 case 0x00: { //backward
Dbee16 2:1feae3cb6731 103 motor_r.write( r_result < 0 ? 0 : r_result > 1 ? 1 : r_result);
Dbee16 2:1feae3cb6731 104 motor_l.write( l_result < 0 ? 0 : l_result > 1 ? 1 : l_result);
Dbee16 2:1feae3cb6731 105
cmcmaster 0:59d25bb7f825 106 motor_rf=0;
cmcmaster 0:59d25bb7f825 107 motor_rb=1;
cmcmaster 0:59d25bb7f825 108 motor_lf=0;
cmcmaster 0:59d25bb7f825 109 motor_lb=1;
cmcmaster 0:59d25bb7f825 110 }
Dbee16 2:1feae3cb6731 111 case 0x01: { //spin left -- Right forward, left backward
Dbee16 2:1feae3cb6731 112 motor_r.write( r_result < 0 ? 0 : r_result > 1 ? 1 : r_result);
Dbee16 2:1feae3cb6731 113 motor_l.write( l_result < 0 ? 0 : l_result > 1 ? 1 : l_result);
Dbee16 2:1feae3cb6731 114
Dbee16 2:1feae3cb6731 115 motor_rf=1;
Dbee16 2:1feae3cb6731 116 motor_rb=0;
Dbee16 2:1feae3cb6731 117 motor_lf=0;
Dbee16 2:1feae3cb6731 118 motor_lb=1;
Dbee16 2:1feae3cb6731 119 }
Dbee16 2:1feae3cb6731 120 case 0x10: { //spin right
Dbee16 2:1feae3cb6731 121 motor_r.write( r_result < 0 ? 0 : r_result > 1 ? 1 : r_result);
Dbee16 2:1feae3cb6731 122 motor_l.write( l_result < 0 ? 0 : l_result > 1 ? 1 : l_result);
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 2:1feae3cb6731 128 }
cmcmaster 0:59d25bb7f825 129 }
cmcmaster 0:59d25bb7f825 130 }
cmcmaster 5:22d7fee2e26e 131
cmcmaster 5:22d7fee2e26e 132 void change_direction( int sensor_values) //right hand sensors only
cmcmaster 5:22d7fee2e26e 133 {
cmcmaster 5:22d7fee2e26e 134 //Line_right = !Line_right;
cmcmaster 5:22d7fee2e26e 135 switch( sensor_values ) {
cmcmaster 5:22d7fee2e26e 136 case 0x00: { //no lines robot is lost!!!!
cmcmaster 5:22d7fee2e26e 137 set_direction(0x11, 0.0, 0.0);
cmcmaster 5:22d7fee2e26e 138 wait(1);
cmcmaster 5:22d7fee2e26e 139 break;
cmcmaster 5:22d7fee2e26e 140 }
cmcmaster 5:22d7fee2e26e 141 case 0x01: { //to far left turn right 0001
Dbee16 6:dd0a91c2994f 142 set_direction(0x11, 0.5, 0.0); //this doesn't turn right; this goes straight at half speed
cmcmaster 5:22d7fee2e26e 143 wait(1);
cmcmaster 5:22d7fee2e26e 144 Line_right = !Line_right;
cmcmaster 5:22d7fee2e26e 145 break;
cmcmaster 5:22d7fee2e26e 146 }
cmcmaster 5:22d7fee2e26e 147 case 0x02: { //left turn right 0010
cmcmaster 5:22d7fee2e26e 148 set_direction(0x11, 0.0, 0.0);
cmcmaster 5:22d7fee2e26e 149 wait(1);
cmcmaster 5:22d7fee2e26e 150 break;
cmcmaster 5:22d7fee2e26e 151 }
cmcmaster 5:22d7fee2e26e 152 case 0x03: { //far left turn right 0011
cmcmaster 5:22d7fee2e26e 153 set_direction(0x11, 0.0, 0.0);
cmcmaster 5:22d7fee2e26e 154 wait(1);
cmcmaster 5:22d7fee2e26e 155 break;
cmcmaster 5:22d7fee2e26e 156 }
cmcmaster 5:22d7fee2e26e 157 case 0x4: { //right turn left 0100
cmcmaster 5:22d7fee2e26e 158 set_direction(0x11, 0.0, 0.0);
cmcmaster 5:22d7fee2e26e 159 wait(1);
cmcmaster 5:22d7fee2e26e 160 break;
cmcmaster 5:22d7fee2e26e 161 }
cmcmaster 5:22d7fee2e26e 162 case 0x5: { //unknown ** 0101
cmcmaster 5:22d7fee2e26e 163 set_direction(0x11, 0.0, 0.0);
cmcmaster 5:22d7fee2e26e 164 break;
cmcmaster 5:22d7fee2e26e 165 }
cmcmaster 5:22d7fee2e26e 166 case 0x6: { //perfect 0110
cmcmaster 5:22d7fee2e26e 167 set_direction(0x11, 0.0, 0.0);
cmcmaster 5:22d7fee2e26e 168 wait(1);
cmcmaster 5:22d7fee2e26e 169 break;
cmcmaster 5:22d7fee2e26e 170 }
cmcmaster 5:22d7fee2e26e 171 case 0x7: {//corner junction or plate 0111
cmcmaster 5:22d7fee2e26e 172 set_direction(0x00, 0.0, 0.0);
cmcmaster 5:22d7fee2e26e 173 wait(1);
cmcmaster 5:22d7fee2e26e 174 break;
cmcmaster 5:22d7fee2e26e 175 }
cmcmaster 5:22d7fee2e26e 176 case 0x8: {//to far right turn left 1000
cmcmaster 5:22d7fee2e26e 177 set_direction(0x11, 0.0, 0.0);
cmcmaster 5:22d7fee2e26e 178 wait(1);
cmcmaster 5:22d7fee2e26e 179 break;
cmcmaster 5:22d7fee2e26e 180 }
cmcmaster 5:22d7fee2e26e 181 case 0x9: { //unknown 1001
cmcmaster 5:22d7fee2e26e 182 set_direction(0x11, 0.0, 0.0);
cmcmaster 5:22d7fee2e26e 183 wait(1);
cmcmaster 5:22d7fee2e26e 184 break;
cmcmaster 5:22d7fee2e26e 185 }
cmcmaster 5:22d7fee2e26e 186 case 0xA: { //unknown 1010
cmcmaster 5:22d7fee2e26e 187 set_direction(0x11, 0.0, 0.0);
cmcmaster 5:22d7fee2e26e 188 break;
cmcmaster 5:22d7fee2e26e 189 }
cmcmaster 5:22d7fee2e26e 190 case 0xB: { //unknown 1011
cmcmaster 5:22d7fee2e26e 191 set_direction(0x11, 0.0, 0.0);
cmcmaster 5:22d7fee2e26e 192 wait(1);
cmcmaster 5:22d7fee2e26e 193 break;
cmcmaster 5:22d7fee2e26e 194 }
cmcmaster 5:22d7fee2e26e 195 case 0xC: { //far right turn left 1100
cmcmaster 5:22d7fee2e26e 196 set_direction(0x11, 0.0, 0.0);
cmcmaster 5:22d7fee2e26e 197 wait(1);
cmcmaster 5:22d7fee2e26e 198 break;
cmcmaster 5:22d7fee2e26e 199 }
cmcmaster 5:22d7fee2e26e 200 case 0xD: { //unknown 1101
cmcmaster 5:22d7fee2e26e 201 set_direction(0x11, 0.0, 0.0);
cmcmaster 5:22d7fee2e26e 202 break;
cmcmaster 5:22d7fee2e26e 203 }
cmcmaster 5:22d7fee2e26e 204 case 0xE: { //corner or plate 1110
cmcmaster 5:22d7fee2e26e 205 set_direction(0x00, 0.0, 0.0);
cmcmaster 5:22d7fee2e26e 206 wait(1);
cmcmaster 5:22d7fee2e26e 207 break;
cmcmaster 5:22d7fee2e26e 208 }
cmcmaster 5:22d7fee2e26e 209 case 0xF: { //corner 1111
cmcmaster 5:22d7fee2e26e 210 set_direction(0x11, 0.0, 0.0);
cmcmaster 5:22d7fee2e26e 211 wait(1);
cmcmaster 5:22d7fee2e26e 212 break;
cmcmaster 5:22d7fee2e26e 213 }
cmcmaster 5:22d7fee2e26e 214 }
cmcmaster 5:22d7fee2e26e 215 }
cmcmaster 0:59d25bb7f825 216 int main()
cmcmaster 0:59d25bb7f825 217 {
Dbee16 2:1feae3cb6731 218 //Set PWM frequency to 1000Hz
cmcmaster 0:59d25bb7f825 219 motor_l.period( 1.0f / (float) PWM_FREQ);
cmcmaster 0:59d25bb7f825 220 motor_r.period( 1.0f / (float) PWM_FREQ);
cmcmaster 0:59d25bb7f825 221 //Initialise direction to nothing.
cmcmaster 0:59d25bb7f825 222 motor_rf=0;
cmcmaster 0:59d25bb7f825 223 motor_rb=0;
cmcmaster 0:59d25bb7f825 224 motor_lf=0;
cmcmaster 0:59d25bb7f825 225 motor_lb=0;
cmcmaster 5:22d7fee2e26e 226
cmcmaster 0:59d25bb7f825 227
Dbee16 1:2bab3a0bc3bc 228 while(1) {
Dbee16 1:2bab3a0bc3bc 229 led = !led;
cmcmaster 5:22d7fee2e26e 230 int values;
Dbee16 2:1feae3cb6731 231
cmcmaster 5:22d7fee2e26e 232 values = sensor_read();
Dbee16 2:1feae3cb6731 233
cmcmaster 5:22d7fee2e26e 234 change_direction(values);
cmcmaster 5:22d7fee2e26e 235
Dbee16 1:2bab3a0bc3bc 236 }
cmcmaster 5:22d7fee2e26e 237 }
cmcmaster 5:22d7fee2e26e 238
cmcmaster 5:22d7fee2e26e 239
cmcmaster 0:59d25bb7f825 240
cmcmaster 0:59d25bb7f825 241
cmcmaster 0:59d25bb7f825 242