CCM / Mbed 2 deprecated MCU_Driving

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:   PID QEI mbed

main.cpp@13:94758a76a81a, 2014-05-20 (annotated)

Committer:
sjmcscheepens
Date:
Tue May 20 20:44:09 2014 +0000
Revision:
13:94758a76a81a
Parent:
11:fe09a710265d 
ready for testing;

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