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@15:3f6626f68836, 2015-03-19 (annotated)

Committer:
cmcmaster
Date:
Thu Mar 19 13:40:00 2015 +0000
Revision:
15:3f6626f68836
Parent:
11:f07662c64e26
Child:
17:54da4359134f
Working line detection..

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
cmcmaster 15:3f6626f68836 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 1:2bab3a0bc3bc 30
Dbee16 9:92895704e1a4 31
Dbee16 9:92895704e1a4 32 void set_direction( int direction, float duty_l, float duty_r)
Dbee16 9:92895704e1a4 33 {
Dbee16 2:1feae3cb6731 34 switch( direction ) {
Dbee16 2:1feae3cb6731 35 case 0x11: { //forward
Dbee16 11:f07662c64e26 36 motor_r.write( duty_r);
Dbee16 2:1feae3cb6731 37 //Look up ternary statements -- {condition ? value_if_true : value_if_false} This makes 0<result<1
Dbee16 9:92895704e1a4 38 motor_l.write( duty_l);
Dbee16 2:1feae3cb6731 39
cmcmaster 0:59d25bb7f825 40 motor_rf=1;
cmcmaster 0:59d25bb7f825 41 motor_rb=0;
cmcmaster 0:59d25bb7f825 42 motor_lf=1;
cmcmaster 0:59d25bb7f825 43 motor_lb=0;
Dbee16 11:f07662c64e26 44 blue.printf("Going forward, right:%f ; left:%f\n", duty_r, duty_l);
Dbee16 11:f07662c64e26 45 break;
cmcmaster 0:59d25bb7f825 46 }
Dbee16 2:1feae3cb6731 47 case 0x00: { //backward
Dbee16 9:92895704e1a4 48 motor_r.write( duty_r);
Dbee16 9:92895704e1a4 49 motor_l.write( duty_l);
Dbee16 2:1feae3cb6731 50
cmcmaster 0:59d25bb7f825 51 motor_rf=0;
cmcmaster 0:59d25bb7f825 52 motor_rb=1;
cmcmaster 0:59d25bb7f825 53 motor_lf=0;
cmcmaster 0:59d25bb7f825 54 motor_lb=1;
Dbee16 11:f07662c64e26 55 blue.printf("Going backward, right:%f ; left:%f\n", duty_r, duty_l);
Dbee16 11:f07662c64e26 56 break;
cmcmaster 0:59d25bb7f825 57 }
Dbee16 2:1feae3cb6731 58 case 0x01: { //spin left -- Right forward, left backward
Dbee16 9:92895704e1a4 59 motor_r.write( duty_r);
Dbee16 9:92895704e1a4 60 motor_l.write( duty_l);
Dbee16 2:1feae3cb6731 61
Dbee16 2:1feae3cb6731 62 motor_rf=1;
Dbee16 2:1feae3cb6731 63 motor_rb=0;
Dbee16 2:1feae3cb6731 64 motor_lf=0;
Dbee16 2:1feae3cb6731 65 motor_lb=1;
Dbee16 11:f07662c64e26 66 blue.printf("Spinning Left, right:%f ; left:%f\n", duty_r, duty_l);
Dbee16 11:f07662c64e26 67 break;
Dbee16 2:1feae3cb6731 68 }
Dbee16 2:1feae3cb6731 69 case 0x10: { //spin right
Dbee16 9:92895704e1a4 70 motor_r.write( duty_r);
Dbee16 9:92895704e1a4 71 motor_l.write( duty_l);
Dbee16 2:1feae3cb6731 72
Dbee16 2:1feae3cb6731 73 motor_rf=0;
Dbee16 2:1feae3cb6731 74 motor_rb=1;
Dbee16 2:1feae3cb6731 75 motor_lf=1;
Dbee16 2:1feae3cb6731 76 motor_lb=0;
Dbee16 11:f07662c64e26 77 blue.printf("Spinning Right, right:%f ; left:%f\n", duty_r, duty_l);
Dbee16 11:f07662c64e26 78 break;
Dbee16 2:1feae3cb6731 79 }
cmcmaster 0:59d25bb7f825 80 }
cmcmaster 0:59d25bb7f825 81 }
Dbee16 9:92895704e1a4 82
cmcmaster 15:3f6626f68836 83
cmcmaster 15:3f6626f68836 84 void motor_result(int val)
Dbee16 8:eefbd3880d28 85 {
cmcmaster 15:3f6626f68836 86 led= 0;
cmcmaster 15:3f6626f68836 87 switch(val) {
cmcmaster 15:3f6626f68836 88 case 0: //lost
cmcmaster 15:3f6626f68836 89
cmcmaster 15:3f6626f68836 90 set_direction(0x00, 0,0);
cmcmaster 15:3f6626f68836 91 break;
cmcmaster 15:3f6626f68836 92 case 1:
cmcmaster 15:3f6626f68836 93
cmcmaster 15:3f6626f68836 94 set_direction(0x11, 0.5,0.6);
Dbee16 9:92895704e1a4 95 break;
cmcmaster 15:3f6626f68836 96 case 2:
cmcmaster 15:3f6626f68836 97
cmcmaster 15:3f6626f68836 98 break;
cmcmaster 15:3f6626f68836 99 case 4:
cmcmaster 15:3f6626f68836 100
Dbee16 9:92895704e1a4 101 break;
cmcmaster 15:3f6626f68836 102 case 6:
cmcmaster 15:3f6626f68836 103 set_direction(0x11, 0.5,0.5);
cmcmaster 15:3f6626f68836 104
cmcmaster 15:3f6626f68836 105 break;
cmcmaster 15:3f6626f68836 106 case 8:
cmcmaster 15:3f6626f68836 107
cmcmaster 15:3f6626f68836 108 set_direction(0x11, 0.6,0.5);
Dbee16 9:92895704e1a4 109 break;
cmcmaster 15:3f6626f68836 110 }
cmcmaster 15:3f6626f68836 111 }
cmcmaster 15:3f6626f68836 112 int sensor_read()
cmcmaster 15:3f6626f68836 113 {
cmcmaster 15:3f6626f68836 114 int val=0;
cmcmaster 15:3f6626f68836 115 int x = 0;
cmcmaster 15:3f6626f68836 116 int sens[4] = {0};
cmcmaster 15:3f6626f68836 117 for (x = 0; x < 4; x++) {
cmcmaster 15:3f6626f68836 118 switch(x) {
cmcmaster 15:3f6626f68836 119 case 0:
cmcmaster 15:3f6626f68836 120 sensor = 0x3;
cmcmaster 15:3f6626f68836 121 break;
cmcmaster 15:3f6626f68836 122 case 1:
cmcmaster 15:3f6626f68836 123 sensor = 0x0;
cmcmaster 15:3f6626f68836 124 break;
cmcmaster 15:3f6626f68836 125 case 2:
cmcmaster 15:3f6626f68836 126 sensor = 0x1;
cmcmaster 15:3f6626f68836 127 break;
cmcmaster 15:3f6626f68836 128 case 3:
cmcmaster 15:3f6626f68836 129 sensor = 0x2;
cmcmaster 15:3f6626f68836 130 break;
cmcmaster 15:3f6626f68836 131 case 4:
cmcmaster 15:3f6626f68836 132 sensor = 0x5;
cmcmaster 15:3f6626f68836 133 break;
cmcmaster 15:3f6626f68836 134 case 5:
cmcmaster 15:3f6626f68836 135 sensor = 0x7;
cmcmaster 15:3f6626f68836 136 break;
cmcmaster 15:3f6626f68836 137 case 6:
cmcmaster 15:3f6626f68836 138 sensor = 0x6;
cmcmaster 15:3f6626f68836 139 break;
cmcmaster 15:3f6626f68836 140 case 7:
cmcmaster 15:3f6626f68836 141 sensor = 0x4;
cmcmaster 15:3f6626f68836 142 break;
Dbee16 9:92895704e1a4 143 }
cmcmaster 15:3f6626f68836 144
cmcmaster 15:3f6626f68836 145 sens[x] = input;
cmcmaster 15:3f6626f68836 146 //blue = !blue;
cmcmaster 15:3f6626f68836 147
cmcmaster 15:3f6626f68836 148
cmcmaster 15:3f6626f68836 149
Dbee16 8:eefbd3880d28 150 }
cmcmaster 15:3f6626f68836 151 for(x = 3; x >= 0; x--) {
cmcmaster 15:3f6626f68836 152 val = val << 1;
cmcmaster 15:3f6626f68836 153 val = val + sens[x];
cmcmaster 15:3f6626f68836 154 }
cmcmaster 15:3f6626f68836 155 //blue.printf("%i\n",val);
cmcmaster 15:3f6626f68836 156
cmcmaster 15:3f6626f68836 157 return val;
Dbee16 8:eefbd3880d28 158 }
cmcmaster 0:59d25bb7f825 159 int main()
cmcmaster 0:59d25bb7f825 160 {
Dbee16 9:92895704e1a4 161 level = 0.3; //Analogout Level for black line into comparator
Dbee16 2:1feae3cb6731 162 //Set PWM frequency to 1000Hz
cmcmaster 0:59d25bb7f825 163 motor_l.period( 1.0f / (float) PWM_FREQ);
cmcmaster 0:59d25bb7f825 164 motor_r.period( 1.0f / (float) PWM_FREQ);
cmcmaster 0:59d25bb7f825 165 //Initialise direction to nothing.
cmcmaster 0:59d25bb7f825 166 motor_rf=0;
cmcmaster 0:59d25bb7f825 167 motor_rb=0;
cmcmaster 0:59d25bb7f825 168 motor_lf=0;
cmcmaster 0:59d25bb7f825 169 motor_lb=0;
cmcmaster 15:3f6626f68836 170 int val =0;
Dbee16 9:92895704e1a4 171 while(1) {
cmcmaster 15:3f6626f68836 172 val = sensor_read();
cmcmaster 15:3f6626f68836 173 motor_result(val);
cmcmaster 15:3f6626f68836 174 }
cmcmaster 15:3f6626f68836 175 // while(1) {
cmcmaster 15:3f6626f68836 176 //Working
cmcmaster 15:3f6626f68836 177 /*
cmcmaster 15:3f6626f68836 178 led = !led;
cmcmaster 15:3f6626f68836 179 wait(4);
cmcmaster 15:3f6626f68836 180 blue.printf("Forward, full\n");
cmcmaster 15:3f6626f68836 181 set_direction(0x11, 1.0,1.0);//forward full speed
cmcmaster 15:3f6626f68836 182 wait(4);
cmcmaster 15:3f6626f68836 183 blue.printf("Forward, left half\n");
cmcmaster 15:3f6626f68836 184 led = !led;
cmcmaster 15:3f6626f68836 185 set_direction(0x11, 0.5 ,0); //forward left half
cmcmaster 15:3f6626f68836 186 wait(4);
cmcmaster 15:3f6626f68836 187 blue.printf("Forward, right half\n");
cmcmaster 15:3f6626f68836 188 led = !led;
cmcmaster 15:3f6626f68836 189 set_direction(0x11, 0 ,0.5); //forward right half
cmcmaster 15:3f6626f68836 190 wait(4);
cmcmaster 15:3f6626f68836 191 led = !led;
cmcmaster 15:3f6626f68836 192 blue.printf("stop\n");
cmcmaster 15:3f6626f68836 193 set_direction(0x00, 0 ,0); //stop
cmcmaster 15:3f6626f68836 194 wait(4);
cmcmaster 15:3f6626f68836 195 led = !led;
cmcmaster 15:3f6626f68836 196 blue.printf("Back, full\n");
cmcmaster 15:3f6626f68836 197 set_direction(0x00, 1.0 ,1.0); //backward full speed
cmcmaster 15:3f6626f68836 198 wait(4);
cmcmaster 15:3f6626f68836 199 led = !led;
cmcmaster 15:3f6626f68836 200 blue.printf("Back, left\n");
cmcmaster 15:3f6626f68836 201 set_direction(0x00, 0.5 ,0);
cmcmaster 15:3f6626f68836 202 wait(4);
cmcmaster 15:3f6626f68836 203 blue.printf("Back, right\n");
cmcmaster 15:3f6626f68836 204 led = !led;
cmcmaster 15:3f6626f68836 205 set_direction(0x00, 0 ,0.5);
Dbee16 11:f07662c64e26 206
cmcmaster 15:3f6626f68836 207 */
Dbee16 11:f07662c64e26 208
cmcmaster 15:3f6626f68836 209 //Sensor Code + Motor Test
cmcmaster 15:3f6626f68836 210 //int values = 0;
cmcmaster 15:3f6626f68836 211 // values = sensor_read();
cmcmaster 15:3f6626f68836 212 // change_direction(values);
Dbee16 9:92895704e1a4 213
cmcmaster 15:3f6626f68836 214 //}
Dbee16 9:92895704e1a4 215 }
Dbee16 8:eefbd3880d28 216
Dbee16 8:eefbd3880d28 217
Dbee16 8:eefbd3880d28 218
Dbee16 8:eefbd3880d28 219
Dbee16 8:eefbd3880d28 220