CCM / Mbed 2 deprecated MCU_Driving

CCM Robot 2014: MCU for controlling the driving

Dependencies:   PID QEI mbed

main.cpp@8:68e6dc78d00d, 2014-05-01 (annotated)

Committer:
noxxos
Date:
Thu May 01 00:25:56 2014 +0000
Revision:
8:68e6dc78d00d
Parent:
5:f094d6e7d4d1
Child:
9:ae8acce96a4e
I2C slave code added

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