CCM
/
MCU_Driving
CCM Robot 2014: MCU for controlling the driving
main.cpp
- Committer:
- sjmcscheepens
- Date:
- 2014-05-20
- Revision:
- 15:b2f670dc250a
- Parent:
- 14:303adb7e1519
File content as of revision 15:b2f670dc250a:
#include "mbed.h" #include "PwmOut.h" #include "Timer.h" #include "defines.h" //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ //problems to still solve: //-Who sets distance and angle back to 0? // //to drive a certain distance set the float distance to the desired meters //to turn set angle to desired degrees rotation with respect to current situation // // // //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ //PIN declarations PwmOut l_wheel_1(p25); PwmOut l_wheel_2(p26); PwmOut r_wheel_1(p5); PwmOut r_wheel_2(p6); DigitalOut drive_enable_l(p24); DigitalOut drive_enable_r(p7); // 2e op p7~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~is this a good pin? DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); I2CSlave slave(p28, p27); Timer timer; int state = 1; float distance = 0; //distance to drive in m float dist_time = 0; float meter_time = 2; //seconds for driving a meter float end_time = 0; float angle = 0; float turn_time = 0; float degree_time = 0.5; //seconds for turning 1 degree float turn_end_time = 0; //value declarations float dc_drive = 0.1;// change for higher speeds float dc_low = 0; float dc_high = 1; //function declarations void showstate(void); // implement communication in these functions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~MARCO~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ bool ask_bottle(void) { return 1; } int ask_angle(void) { return 0; } int ask_distance(void) { return 10; } void sm_drive() { led4 = !led4; showstate(); switch(state) { case 1: //standstill // motoren stil l_wheel_1.write(dc_low); l_wheel_2.write(dc_low); r_wheel_1.write(dc_low); r_wheel_2.write(dc_low); //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~`ZHE MAGIK BRAINZZZ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ if( ask_bottle() ) // if bottle is seen: work< if not: turn) { angle = ask_angle(); if( angle == 0) { distance = ask_distance(); // add extra grabbing distance? } } else { angle = 45; distance = 0; } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ZHE END OF ZHE BRAINZZZ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ //state transitions if(distance > 0) { state = 2; timer.start(); dist_time = distance * meter_time; end_time = timer.read() + dist_time; } if(distance < 0) { state = 3; timer.start(); dist_time = -distance * meter_time; end_time = timer.read() + dist_time; } if(angle > 0) { state = 4; timer.start(); turn_time = angle * degree_time; turn_end_time = timer.read() + turn_time; } if(angle < 0) { state = 5; timer.start(); turn_time = -angle * degree_time; turn_end_time = timer.read() + turn_time; } break; case 2: //drive forward l_wheel_1.write(dc_drive); l_wheel_2.write(dc_low); r_wheel_1.write(dc_drive); r_wheel_2.write(dc_low); //check time of clock if(timer.read() >= end_time || distance == 0) { state = 1; timer.stop(); distance = 0; } break; case 3: //drive backward l_wheel_1.write(dc_low); l_wheel_2.write(dc_drive); r_wheel_1.write(dc_low); r_wheel_2.write(dc_drive); //check time of clock if(timer.read() >= end_time || distance == 0) { state = 1; timer.stop(); distance = 0; } break; case 4: // turn left l_wheel_1.write(dc_low); l_wheel_2.write(dc_drive); r_wheel_1.write(dc_drive); r_wheel_2.write(dc_low); if(timer.read() >= turn_end_time || angle ==0) { state = 1; timer.stop(); angle = 0; } break; case 5: //turn right l_wheel_1.write(dc_drive); l_wheel_2.write(dc_low); r_wheel_1.write(dc_low); r_wheel_2.write(dc_drive); if(timer.read() >= turn_end_time || angle == 0) { state = 1; timer.stop(); angle = 0; } break; default: state = 1; break; } } void showstate() { switch(state) { case 1: led1 = 1; led2 = 0; led3 = 0; led4 = 0; break; case 2: led1 = 0; led2 = 1; led3 = 0; led4 = 0; break; case 3: led1 = 1; led2 = 1; led3 = 0; led4 = 0; break; case 4: led1 = 0; led2 = 0; led3 = 1; led4 = 0; break; case 5: led1 = 1; led2 = 0; led3 = 1; led4 = 0; break; } } int main() { l_wheel_1.period(0.00015); r_wheel_1.period(0.00001); drive_enable_l = 1; drive_enable_r = 1; slave.address(I2C_ADDR_SLAVE1); char buf[10]; int j = 0; slave.address(I2C_ADDR_SLAVE1); while(1) { j++; if( j > 5000 ) { printf("%f\n\r", timer.read()); j = 0; } sm_drive(); int i = slave.receive(); switch (i) { case I2CSlave::ReadAddressed: slave.write("hallo\n\r", strlen("hallo\n\r") + 1); // Includes null char printf("Write Slave!\n\r"); break; case I2CSlave::WriteGeneral: slave.read(buf, 10); printf("Read G: %i\n\r", buf); break; case I2CSlave::WriteAddressed: slave.read(buf, 10); switch(buf[0]) { case I2C_COMMAND_STOP: distance = 0; angle = 0; break; case I2C_COMMAND_DRIVE: distance = buf[1] - 128; printf("COMMAND: DRIVE: %i meters\n\r", buf[1] - 128); break; case I2C_COMMAND_ROTATE: angle = buf[1] - 128; printf("COMMAND: ROTATE: %i degrees\n\r", buf[1] - 128); } break; } for(int i = 0; i < 10; i++) buf[i] = 0; // Clear buffer } }