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@9:92895704e1a4, 2015-03-12 (annotated)

Committer:
Dbee16
Date:
Thu Mar 12 14:44:56 2015 +0000
Revision:
9:92895704e1a4
Parent:
8:eefbd3880d28
Child:
10:2970279fce70
Child:
11:f07662c64e26
This is the sensor testing stuff. Not working, there's a problem with the set_direction(). It's probably due to the input being 0x00 and it might just be ignoring it

Who changed what in which revision?

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