CCM / Mbed 2 deprecated MCU_Driving

CCM Robot 2014: MCU for controlling the driving

Dependencies:   PID QEI mbed

main.cpp@15:b2f670dc250a, 2014-05-20 (annotated)

Committer:
sjmcscheepens
Date:
Tue May 20 21:25:09 2014 +0000
Revision:
15:b2f670dc250a
Parent:
14:303adb7e1519 
revision 2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tweaker1331 0:96a538512ba4 1 #include "mbed.h"
sjmcscheepens 2:d078dc23ebaa 2 #include "PwmOut.h"
sjmcscheepens 4:c8461418ac92 3 #include "Timer.h"
noxxos 10:0fdeecaf366d 4 #include "defines.h"
sjmcscheepens 2:d078dc23ebaa 5
sjmcscheepens 5:f094d6e7d4d1 6 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
sjmcscheepens 5:f094d6e7d4d1 7 //problems to still solve:
sjmcscheepens 5:f094d6e7d4d1 8 //-Who sets distance and angle back to 0?
sjmcscheepens 5:f094d6e7d4d1 9 //
sjmcscheepens 6:bc58bd87122e 10 //to drive a certain distance set the float distance to the desired meters
sjmcscheepens 6:bc58bd87122e 11 //to turn set angle to desired degrees rotation with respect to current situation
sjmcscheepens 5:f094d6e7d4d1 12 //
sjmcscheepens 5:f094d6e7d4d1 13 //
sjmcscheepens 5:f094d6e7d4d1 14 //
sjmcscheepens 5:f094d6e7d4d1 15 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
sjmcscheepens 5:f094d6e7d4d1 16
sjmcscheepens 2:d078dc23ebaa 17 //PIN declarations
sjmcscheepens 2:d078dc23ebaa 18
noxxos 12:601f8161a441 19 PwmOut l_wheel_1(p25);
noxxos 12:601f8161a441 20 PwmOut l_wheel_2(p26);
noxxos 12:601f8161a441 21 PwmOut r_wheel_1(p5);
noxxos 12:601f8161a441 22 PwmOut r_wheel_2(p6);
sjmcscheepens 5:f094d6e7d4d1 23
noxxos 12:601f8161a441 24 DigitalOut drive_enable_l(p24);
noxxos 12:601f8161a441 25 DigitalOut drive_enable_r(p7); // 2e op p7~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~is this a good pin?
sjmcscheepens 2:d078dc23ebaa 26 DigitalOut led1(LED1);
sjmcscheepens 2:d078dc23ebaa 27 DigitalOut led2(LED2);
sjmcscheepens 2:d078dc23ebaa 28 DigitalOut led3(LED3);
sjmcscheepens 2:d078dc23ebaa 29 DigitalOut led4(LED4);
sjmcscheepens 2:d078dc23ebaa 30
noxxos 10:0fdeecaf366d 31 I2CSlave slave(p28, p27);
noxxos 10:0fdeecaf366d 32
sjmcscheepens 4:c8461418ac92 33 Timer timer;
sjmcscheepens 5:f094d6e7d4d1 34 int state = 1;
sjmcscheepens 4:c8461418ac92 35 float distance = 0; //distance to drive in m
sjmcscheepens 4:c8461418ac92 36 float dist_time = 0;
sjmcscheepens 5:f094d6e7d4d1 37 float meter_time = 2; //seconds for driving a meter
sjmcscheepens 4:c8461418ac92 38 float end_time = 0;
sjmcscheepens 4:c8461418ac92 39
sjmcscheepens 4:c8461418ac92 40 float angle = 0;
sjmcscheepens 4:c8461418ac92 41 float turn_time = 0;
sjmcscheepens 5:f094d6e7d4d1 42 float degree_time = 0.5; //seconds for turning 1 degree
sjmcscheepens 4:c8461418ac92 43 float turn_end_time = 0;
sjmcscheepens 3:05d62b46b32c 44
sjmcscheepens 2:d078dc23ebaa 45 //value declarations
sjmcscheepens 7:8281928d252d 46 float dc_drive = 0.1;// change for higher speeds
sjmcscheepens 5:f094d6e7d4d1 47 float dc_low = 0;
sjmcscheepens 5:f094d6e7d4d1 48 float dc_high = 1;
sjmcscheepens 2:d078dc23ebaa 49
sjmcscheepens 3:05d62b46b32c 50 //function declarations
sjmcscheepens 3:05d62b46b32c 51 void showstate(void);
tweaker1331 0:96a538512ba4 52
sjmcscheepens 14:303adb7e1519 53 // implement communication in these functions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~MARCO~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
sjmcscheepens 14:303adb7e1519 54
sjmcscheepens 14:303adb7e1519 55 bool ask_bottle(void)
sjmcscheepens 14:303adb7e1519 56 {
sjmcscheepens 15:b2f670dc250a 57 return 1;
sjmcscheepens 14:303adb7e1519 58 }
sjmcscheepens 14:303adb7e1519 59
sjmcscheepens 14:303adb7e1519 60 int ask_angle(void)
sjmcscheepens 14:303adb7e1519 61 {
sjmcscheepens 15:b2f670dc250a 62 return 0;
sjmcscheepens 14:303adb7e1519 63 }
sjmcscheepens 14:303adb7e1519 64
sjmcscheepens 14:303adb7e1519 65 int ask_distance(void)
sjmcscheepens 14:303adb7e1519 66 {
sjmcscheepens 15:b2f670dc250a 67 return 10;
sjmcscheepens 14:303adb7e1519 68 }
sjmcscheepens 14:303adb7e1519 69
sjmcscheepens 3:05d62b46b32c 70 void sm_drive()
sjmcscheepens 3:05d62b46b32c 71 {
sjmcscheepens 5:f094d6e7d4d1 72 led4 = !led4;
sjmcscheepens 3:05d62b46b32c 73 showstate();
sjmcscheepens 3:05d62b46b32c 74 switch(state)
sjmcscheepens 3:05d62b46b32c 75 {
sjmcscheepens 5:f094d6e7d4d1 76 case 1: //standstill
sjmcscheepens 3:05d62b46b32c 77
sjmcscheepens 3:05d62b46b32c 78 // motoren stil
sjmcscheepens 5:f094d6e7d4d1 79 l_wheel_1.write(dc_low);
sjmcscheepens 5:f094d6e7d4d1 80 l_wheel_2.write(dc_low);
sjmcscheepens 5:f094d6e7d4d1 81 r_wheel_1.write(dc_low);
sjmcscheepens 5:f094d6e7d4d1 82 r_wheel_2.write(dc_low);
sjmcscheepens 4:c8461418ac92 83
sjmcscheepens 14:303adb7e1519 84 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~`ZHE MAGIK BRAINZZZ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
sjmcscheepens 14:303adb7e1519 85
sjmcscheepens 14:303adb7e1519 86 if( ask_bottle() ) // if bottle is seen: work< if not: turn)
sjmcscheepens 14:303adb7e1519 87 {
sjmcscheepens 14:303adb7e1519 88 angle = ask_angle();
sjmcscheepens 14:303adb7e1519 89 if( angle == 0)
sjmcscheepens 14:303adb7e1519 90 {
sjmcscheepens 14:303adb7e1519 91 distance = ask_distance(); // add extra grabbing distance?
sjmcscheepens 14:303adb7e1519 92 }
sjmcscheepens 14:303adb7e1519 93 } else
sjmcscheepens 14:303adb7e1519 94 {
sjmcscheepens 14:303adb7e1519 95 angle = 45;
sjmcscheepens 14:303adb7e1519 96 distance = 0;
sjmcscheepens 14:303adb7e1519 97 }
sjmcscheepens 14:303adb7e1519 98
sjmcscheepens 14:303adb7e1519 99
sjmcscheepens 14:303adb7e1519 100 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ZHE END OF ZHE BRAINZZZ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
sjmcscheepens 14:303adb7e1519 101
sjmcscheepens 3:05d62b46b32c 102 //state transitions
sjmcscheepens 6:bc58bd87122e 103 if(distance > 0)
sjmcscheepens 6:bc58bd87122e 104 {
sjmcscheepens 6:bc58bd87122e 105 state = 2;
sjmcscheepens 6:bc58bd87122e 106 timer.start();
sjmcscheepens 6:bc58bd87122e 107 dist_time = distance * meter_time;
sjmcscheepens 6:bc58bd87122e 108 end_time = timer.read() + dist_time;
sjmcscheepens 6:bc58bd87122e 109 }
sjmcscheepens 6:bc58bd87122e 110 if(distance < 0)
sjmcscheepens 6:bc58bd87122e 111 {
sjmcscheepens 6:bc58bd87122e 112 state = 3;
sjmcscheepens 6:bc58bd87122e 113 timer.start();
sjmcscheepens 6:bc58bd87122e 114 dist_time = -distance * meter_time;
sjmcscheepens 6:bc58bd87122e 115 end_time = timer.read() + dist_time;
sjmcscheepens 6:bc58bd87122e 116 }
sjmcscheepens 6:bc58bd87122e 117
sjmcscheepens 6:bc58bd87122e 118 if(angle > 0)
sjmcscheepens 6:bc58bd87122e 119 {
sjmcscheepens 6:bc58bd87122e 120 state = 4;
sjmcscheepens 6:bc58bd87122e 121 timer.start();
sjmcscheepens 6:bc58bd87122e 122 turn_time = angle * degree_time;
sjmcscheepens 6:bc58bd87122e 123 turn_end_time = timer.read() + turn_time;
sjmcscheepens 6:bc58bd87122e 124 }
sjmcscheepens 6:bc58bd87122e 125 if(angle < 0)
sjmcscheepens 6:bc58bd87122e 126 {
sjmcscheepens 6:bc58bd87122e 127 state = 5;
sjmcscheepens 6:bc58bd87122e 128 timer.start();
sjmcscheepens 6:bc58bd87122e 129 turn_time = -angle * degree_time;
sjmcscheepens 6:bc58bd87122e 130 turn_end_time = timer.read() + turn_time;
sjmcscheepens 6:bc58bd87122e 131 }
sjmcscheepens 5:f094d6e7d4d1 132
sjmcscheepens 5:f094d6e7d4d1 133
sjmcscheepens 5:f094d6e7d4d1 134 break;
sjmcscheepens 5:f094d6e7d4d1 135
sjmcscheepens 5:f094d6e7d4d1 136 case 2: //drive forward
sjmcscheepens 5:f094d6e7d4d1 137 l_wheel_1.write(dc_drive);
noxxos 12:601f8161a441 138 l_wheel_2.write(dc_low);
sjmcscheepens 5:f094d6e7d4d1 139 r_wheel_1.write(dc_drive);
noxxos 12:601f8161a441 140 r_wheel_2.write(dc_low);
sjmcscheepens 5:f094d6e7d4d1 141 //check time of clock
noxxos 12:601f8161a441 142 if(timer.read() >= end_time || distance == 0)
sjmcscheepens 4:c8461418ac92 143 {
sjmcscheepens 5:f094d6e7d4d1 144 state = 1;
sjmcscheepens 5:f094d6e7d4d1 145 timer.stop();
sjmcscheepens 5:f094d6e7d4d1 146 distance = 0;
sjmcscheepens 4:c8461418ac92 147 }
sjmcscheepens 3:05d62b46b32c 148 break;
sjmcscheepens 3:05d62b46b32c 149
sjmcscheepens 5:f094d6e7d4d1 150 case 3: //drive backward
noxxos 12:601f8161a441 151 l_wheel_1.write(dc_low);
sjmcscheepens 5:f094d6e7d4d1 152 l_wheel_2.write(dc_drive);
noxxos 12:601f8161a441 153 r_wheel_1.write(dc_low);
sjmcscheepens 5:f094d6e7d4d1 154 r_wheel_2.write(dc_drive);
sjmcscheepens 4:c8461418ac92 155 //check time of clock
noxxos 12:601f8161a441 156 if(timer.read() >= end_time || distance == 0)
sjmcscheepens 4:c8461418ac92 157 {
sjmcscheepens 5:f094d6e7d4d1 158 state = 1;
sjmcscheepens 4:c8461418ac92 159 timer.stop();
sjmcscheepens 5:f094d6e7d4d1 160 distance = 0;
sjmcscheepens 4:c8461418ac92 161 }
sjmcscheepens 4:c8461418ac92 162 break;
sjmcscheepens 4:c8461418ac92 163
sjmcscheepens 5:f094d6e7d4d1 164 case 4: // turn left
noxxos 12:601f8161a441 165 l_wheel_1.write(dc_low);
sjmcscheepens 5:f094d6e7d4d1 166 l_wheel_2.write(dc_drive);
sjmcscheepens 5:f094d6e7d4d1 167 r_wheel_1.write(dc_drive);
noxxos 12:601f8161a441 168 r_wheel_2.write(dc_low);
sjmcscheepens 5:f094d6e7d4d1 169
noxxos 12:601f8161a441 170 if(timer.read() >= turn_end_time || angle ==0)
sjmcscheepens 4:c8461418ac92 171 {
sjmcscheepens 5:f094d6e7d4d1 172 state = 1;
sjmcscheepens 4:c8461418ac92 173 timer.stop();
sjmcscheepens 5:f094d6e7d4d1 174 angle = 0;
sjmcscheepens 4:c8461418ac92 175 }
sjmcscheepens 4:c8461418ac92 176 break;
sjmcscheepens 4:c8461418ac92 177
sjmcscheepens 5:f094d6e7d4d1 178 case 5: //turn right
sjmcscheepens 5:f094d6e7d4d1 179 l_wheel_1.write(dc_drive);
noxxos 12:601f8161a441 180 l_wheel_2.write(dc_low);
noxxos 12:601f8161a441 181 r_wheel_1.write(dc_low);
sjmcscheepens 5:f094d6e7d4d1 182 r_wheel_2.write(dc_drive);
noxxos 12:601f8161a441 183 if(timer.read() >= turn_end_time || angle == 0)
sjmcscheepens 4:c8461418ac92 184 {
sjmcscheepens 5:f094d6e7d4d1 185 state = 1;
sjmcscheepens 4:c8461418ac92 186 timer.stop();
sjmcscheepens 5:f094d6e7d4d1 187 angle = 0;
sjmcscheepens 4:c8461418ac92 188 }
sjmcscheepens 3:05d62b46b32c 189 break;
sjmcscheepens 6:bc58bd87122e 190
sjmcscheepens 6:bc58bd87122e 191 default:
sjmcscheepens 6:bc58bd87122e 192 state = 1;
sjmcscheepens 6:bc58bd87122e 193 break;
sjmcscheepens 3:05d62b46b32c 194 }
sjmcscheepens 3:05d62b46b32c 195 }
sjmcscheepens 3:05d62b46b32c 196
sjmcscheepens 3:05d62b46b32c 197
sjmcscheepens 3:05d62b46b32c 198
sjmcscheepens 3:05d62b46b32c 199 void showstate()
sjmcscheepens 2:d078dc23ebaa 200 {
sjmcscheepens 5:f094d6e7d4d1 201 switch(state)
sjmcscheepens 5:f094d6e7d4d1 202 {
sjmcscheepens 5:f094d6e7d4d1 203 case 1:
sjmcscheepens 5:f094d6e7d4d1 204 led1 = 1;
sjmcscheepens 5:f094d6e7d4d1 205 led2 = 0;
sjmcscheepens 5:f094d6e7d4d1 206 led3 = 0;
sjmcscheepens 5:f094d6e7d4d1 207 led4 = 0;
sjmcscheepens 5:f094d6e7d4d1 208 break;
sjmcscheepens 5:f094d6e7d4d1 209 case 2:
sjmcscheepens 5:f094d6e7d4d1 210 led1 = 0;
sjmcscheepens 5:f094d6e7d4d1 211 led2 = 1;
sjmcscheepens 5:f094d6e7d4d1 212 led3 = 0;
sjmcscheepens 5:f094d6e7d4d1 213 led4 = 0;
sjmcscheepens 5:f094d6e7d4d1 214 break;
sjmcscheepens 5:f094d6e7d4d1 215 case 3:
sjmcscheepens 5:f094d6e7d4d1 216 led1 = 1;
sjmcscheepens 5:f094d6e7d4d1 217 led2 = 1;
sjmcscheepens 5:f094d6e7d4d1 218 led3 = 0;
sjmcscheepens 5:f094d6e7d4d1 219 led4 = 0;
sjmcscheepens 5:f094d6e7d4d1 220 break;
sjmcscheepens 5:f094d6e7d4d1 221 case 4:
sjmcscheepens 5:f094d6e7d4d1 222 led1 = 0;
sjmcscheepens 5:f094d6e7d4d1 223 led2 = 0;
sjmcscheepens 5:f094d6e7d4d1 224 led3 = 1;
sjmcscheepens 5:f094d6e7d4d1 225 led4 = 0;
sjmcscheepens 5:f094d6e7d4d1 226 break;
sjmcscheepens 5:f094d6e7d4d1 227 case 5:
sjmcscheepens 5:f094d6e7d4d1 228 led1 = 1;
sjmcscheepens 5:f094d6e7d4d1 229 led2 = 0;
sjmcscheepens 5:f094d6e7d4d1 230 led3 = 1;
sjmcscheepens 5:f094d6e7d4d1 231 led4 = 0;
sjmcscheepens 5:f094d6e7d4d1 232 break;
sjmcscheepens 5:f094d6e7d4d1 233 }
sjmcscheepens 2:d078dc23ebaa 234 }
tweaker1331 0:96a538512ba4 235
sjmcscheepens 2:d078dc23ebaa 236 int main()
sjmcscheepens 2:d078dc23ebaa 237 {
noxxos 10:0fdeecaf366d 238
noxxos 12:601f8161a441 239 l_wheel_1.period(0.00015);
sjmcscheepens 5:f094d6e7d4d1 240 r_wheel_1.period(0.00001);
noxxos 12:601f8161a441 241 drive_enable_l = 1;
noxxos 12:601f8161a441 242 drive_enable_r = 1;
noxxos 10:0fdeecaf366d 243
noxxos 11:fe09a710265d 244 slave.address(I2C_ADDR_SLAVE1);
sjmcscheepens 5:f094d6e7d4d1 245
noxxos 10:0fdeecaf366d 246 char buf[10];
noxxos 12:601f8161a441 247 int j = 0;
noxxos 10:0fdeecaf366d 248
noxxos 10:0fdeecaf366d 249 slave.address(I2C_ADDR_SLAVE1);
sjmcscheepens 5:f094d6e7d4d1 250
sjmcscheepens 5:f094d6e7d4d1 251 while(1)
sjmcscheepens 5:f094d6e7d4d1 252 {
noxxos 12:601f8161a441 253 j++;
noxxos 12:601f8161a441 254 if( j > 5000 ) {
noxxos 12:601f8161a441 255 printf("%f\n\r", timer.read());
noxxos 12:601f8161a441 256 j = 0;
noxxos 12:601f8161a441 257 }
noxxos 12:601f8161a441 258
sjmcscheepens 5:f094d6e7d4d1 259 sm_drive();
noxxos 10:0fdeecaf366d 260
noxxos 10:0fdeecaf366d 261 int i = slave.receive();
noxxos 10:0fdeecaf366d 262 switch (i) {
noxxos 10:0fdeecaf366d 263 case I2CSlave::ReadAddressed:
noxxos 10:0fdeecaf366d 264 slave.write("hallo\n\r", strlen("hallo\n\r") + 1); // Includes null char
noxxos 10:0fdeecaf366d 265 printf("Write Slave!\n\r");
noxxos 10:0fdeecaf366d 266 break;
noxxos 10:0fdeecaf366d 267 case I2CSlave::WriteGeneral:
noxxos 10:0fdeecaf366d 268 slave.read(buf, 10);
noxxos 10:0fdeecaf366d 269 printf("Read G: %i\n\r", buf);
noxxos 10:0fdeecaf366d 270 break;
noxxos 10:0fdeecaf366d 271 case I2CSlave::WriteAddressed:
noxxos 10:0fdeecaf366d 272 slave.read(buf, 10);
noxxos 10:0fdeecaf366d 273 switch(buf[0]) {
noxxos 10:0fdeecaf366d 274 case I2C_COMMAND_STOP:
noxxos 10:0fdeecaf366d 275 distance = 0;
noxxos 10:0fdeecaf366d 276 angle = 0;
noxxos 10:0fdeecaf366d 277 break;
noxxos 10:0fdeecaf366d 278 case I2C_COMMAND_DRIVE:
noxxos 12:601f8161a441 279 distance = buf[1] - 128;
noxxos 12:601f8161a441 280 printf("COMMAND: DRIVE: %i meters\n\r", buf[1] - 128);
noxxos 10:0fdeecaf366d 281 break;
noxxos 10:0fdeecaf366d 282 case I2C_COMMAND_ROTATE:
noxxos 12:601f8161a441 283 angle = buf[1] - 128;
noxxos 12:601f8161a441 284 printf("COMMAND: ROTATE: %i degrees\n\r", buf[1] - 128);
noxxos 10:0fdeecaf366d 285 }
noxxos 10:0fdeecaf366d 286 break;
noxxos 10:0fdeecaf366d 287 }
noxxos 10:0fdeecaf366d 288 for(int i = 0; i < 10; i++) buf[i] = 0; // Clear buffer
sjmcscheepens 5:f094d6e7d4d1 289 }
sjmcscheepens 5:f094d6e7d4d1 290
sjmcscheepens 2:d078dc23ebaa 291 }