Guillaume Mourozeau
1st Version of LiveXboxRobot project. Robot side.
Dependencies: Motor mbed-rtos mbed
Revision 0:398cc0dda9e7, committed 2016-04-28
- Comitter:
- GuillaumeMrzo
- Date:
- Thu Apr 28 12:01:17 2016 +0000
- Commit message:
- 1st Version of LiveXboxRobot project. Robot side.
Changed in this revision
diff -r 000000000000 -r 398cc0dda9e7 Motor.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motor.lib Thu Apr 28 12:01:17 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/Motor/#f265e441bcd9
diff -r 000000000000 -r 398cc0dda9e7 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Apr 28 12:01:17 2016 +0000
@@ -0,0 +1,375 @@
+#include "mbed.h"
+#include "Motor.h"
+#include "rtos.h"
+#include "stdio.h"
+
+// Motors and speed
+Motor m_r(p24, p17, p18); // pwm, fwd, rev
+Motor m_l(p25, p20, p19); // pwm, fwd, rev
+float s_max = 0.5;
+float s_min = 0.0;
+
+// LEDS
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+// Serial ports
+RawSerial pc(USBTX, USBRX);
+RawSerial xbee(p9, p10);
+char X;
+Mutex xbee_mutex;
+DigitalOut rst1(p8);
+
+// Flags for the buttons
+int flagFW, flagRight, flagLeft, flagRev;
+int flag1, flag2, flag3, flag4;
+int flag_control;
+
+// IR detectors
+AnalogIn IR_right(p15);
+AnalogIn IR_left(p16);
+float dist = 0.2;
+int iR, iL;
+
+
+
+
+// ------------------- Thread Motor ------------------------//
+void thread_Motor(void const *args){
+ while (true) {
+
+ m_r.speed(0);
+ m_l.speed(0);
+
+ int control = 0;
+
+ iR = (int)(100*IR_right);
+ iL = (int)(100*IR_left);
+
+
+ xbee.putc('R');
+ Thread::wait(100);
+ xbee.putc(iR);
+ Thread::wait(100);
+
+ xbee.putc('L');
+ Thread::wait(100);
+ xbee.putc(iL);
+ Thread::wait(100);
+
+
+ // ------- Keyboard-GUI mode ------------------
+ if(control == 0){
+
+ led1 = led2 = led3 = led4 = 0;
+
+ // Push FW button 1st
+ while(flagFW == 1){
+ /*
+ iR = (int)(100*IR_right);
+ iL = (int)(100*IR_left);
+
+
+ xbee.putc('R');
+ Thread::wait(100);
+ xbee.putc(iR);
+ Thread::wait(100);
+
+ xbee.putc('L');
+ Thread::wait(100);
+ xbee.putc(iL);
+ Thread::wait(100); */
+
+
+ led2 = led3 = 1;
+ led1 = led4 = 0;
+
+ if(flagLeft == 1){
+ led1 = 1;
+ m_l.speed(0);
+ pc.printf("left");
+
+ // No obstacle
+ if((IR_right < dist) && (IR_left < dist)){
+ m_r.speed(s_max);
+ }
+ // Obstacle on the left
+ else if((IR_left >= dist)){
+ m_r.speed(0);
+ }
+ }
+ if(flagRight == 1){
+ led4 = 1;
+ m_r.speed(0);
+ pc.printf("right");
+
+ // No obstacle
+ if((IR_right < dist) && (IR_left < dist)){
+ m_l.speed(s_max);
+ }
+ // Obstacle on the right
+ else if((IR_right >= dist)){
+ m_l.speed(0);
+ }
+ }
+ if(flagRev == 1){
+ led1 = led2 = led3 = led4 = 0;
+ m_r.speed(s_min);
+ m_l.speed(s_min);
+ }
+ else{
+ // No obstacle
+ if((IR_right < dist) && (IR_left < dist)){
+ m_r.speed(s_max);
+ m_l.speed(s_max);
+ }
+
+ // Obstacle on the right --> turn left and reduce speed -----------------------------------
+ else if((IR_right >= dist) && (IR_left < dist)){
+ m_r.speed(s_max);
+ m_l.speed(0);
+ }
+
+ // Obstacle on the left --> turn right and reduce speed
+ else if((IR_right < dist) && (IR_left >= dist)){
+ m_r.speed(0);
+ m_l.speed(s_max);
+ }
+
+ // Obstacle in front --> stop ------------------------------------------
+ else if((IR_right >= dist) && (IR_left >= dist)){
+ m_r.speed(0);
+ m_l.speed(0);
+ }
+ }
+ }
+
+ // Push Rev button 1st
+ while(flagRev == 1){
+ led1 = led2 = led3 = led4 = 1;
+ /*
+ iR = (int)(100*IR_right);
+ iL = (int)(100*IR_left);
+
+
+ xbee.putc('R');
+ Thread::wait(100);
+ xbee.putc(iR);
+ Thread::wait(100);
+
+ xbee.putc('L');
+ Thread::wait(100);
+ xbee.putc(iL);
+ Thread::wait(100);*/
+
+
+ if(flagLeft == 1){
+ led1 = 0;
+ m_r.speed(-s_max);
+ m_l.speed(-s_min);
+ }
+ if(flagRight == 1){
+ led4 = 0;
+ m_r.speed(-s_min);
+ m_l.speed(-s_max);
+ }
+ if(flagFW == 1){
+ led1 = led2 = led3 = led4 = 0;
+ m_r.speed(-s_min);
+ m_l.speed(-s_min);
+ }
+ else{
+ m_r.speed(-s_max);
+ m_l.speed(-s_max);
+ }
+ }
+
+ // Push Left button 1st
+ while(flagLeft == 1){
+ led1 = 1;
+ led2 = led3 = led4 = 0;
+
+ /*
+ iR = (int)(100*IR_right);
+ iL = (int)(100*IR_left);
+
+ xbee.putc('R');
+ Thread::wait(100);
+ xbee.putc(iR);
+ Thread::wait(100);
+
+ xbee.putc('L');
+ Thread::wait(100);
+ xbee.putc(iL);
+ Thread::wait(100);*/
+
+ if(flagFW == 1){
+ led2 = led3 = 1;
+ m_l.speed(0);
+ // No obstacle
+ if(IR_left < dist){
+ m_r.speed(s_max);
+ }
+ // Obstacle on the left
+ else{
+ m_r.speed(0);
+ }
+ }
+ if(flagRight == 1){
+ led1 = led2 = led3 = led4 = 0;
+ m_r.speed(s_min);
+ m_l.speed(s_min);
+ }
+ if(flagRev == 1){
+ led2 = led3 = led4 = 1;
+ led1 = 0;
+ m_r.speed(-s_min);
+ m_l.speed(-s_max);
+ }
+ else{
+ m_r.speed(s_max);
+ m_l.speed(-s_max);
+ }
+ }
+
+ // Push Right button 1st
+ while(flagRight == 1){
+ led4 = 1;
+ led1 = led2 = led3 = 0;
+
+ /*
+ iR = (int)(100*IR_right);
+ iL = (int)(100*IR_left);
+
+
+ xbee.putc('R');
+ Thread::wait(100);
+ xbee.putc(iR);
+ Thread::wait(100);
+
+ xbee.putc('L');
+ Thread::wait(100);
+ xbee.putc(iL);
+ Thread::wait(100);*/
+
+ if(flagFW == 1){
+ led2 = led3 = 1;
+ m_r.speed(0);
+ // No obstacle
+ if(IR_right < dist){
+ m_l.speed(s_max);
+ }
+ // Obstacle on the right
+ else{
+ m_l.speed(0);
+ }
+ }
+ if(flagLeft == 1){
+ led1 = led2 = led3 = led4 = 0;
+ m_r.speed(s_min);
+ m_l.speed(s_min);
+ }
+ if(flagRev == 1){
+ led2 = led3 = led1 = 1;
+ led4 = 0;
+ m_r.speed(-s_max);
+ m_l.speed(-s_min);
+ }
+ else{
+ m_r.speed(-s_max);
+ m_l.speed(s_max);
+ }
+ }
+ }
+
+
+ Thread::wait(10);
+ }
+}
+
+
+// -------------------- Callback Xbee receive ---------------------- //
+void xbee_recv(){
+ led1 = !led1;
+ while(xbee.readable()) {
+ xbee_mutex.lock();
+ X = xbee.getc();
+ xbee_mutex.unlock();
+ switch (X) {
+ case '1': //number button 1
+ s_max = 0.3;
+ dist = 0.2;
+ pc.printf("Speed max 1: %.2f\n", s_max);
+ break;
+ case '2': //number button 2
+ s_max = 0.5;
+ dist = 0.2;
+ pc.printf("Speed max 1: %.2f\n", s_max);
+ break;
+ case '3': //number button 3
+ s_max = 0.8;
+ dist = 0.25;
+ pc.printf("Speed max 1: %.2f\n", s_max);
+ break;
+ case '4': //number button 4
+ s_max = 1.0;
+ dist = 0.3;
+ pc.printf("Speed max 1: %.2f\n", s_max);
+ break;
+ case 'U': // key up arrow
+ flagFW = 1;
+ pc.putc(X);
+ break;
+ case 'D': // key 6 down arrow
+ flagRev = 1;
+ pc.putc(X);
+ break;
+ case 'L': // key 7 left arrow
+ flagLeft = 1;
+ pc.putc(X);
+ break;
+ case 'R': //button 8 right arrow
+ flagRight = 1;
+ pc.putc(X);
+ break;
+ case 'A': //button 8 right arrow
+ flagFW = 0;
+ pc.putc(X);
+ break;
+ case 'B': //button 8 right arrow
+ flagRev = 0;
+ pc.putc(X);
+ break;
+ case 'C': //button 8 right arrow
+ flagLeft = 0;
+ pc.putc(X);
+ break;
+ case 'E': //button 8 right arrow
+ flagRight = 0;
+ pc.putc(X);
+ break;
+ default:
+ break;
+ }
+ }
+}
+
+
+// -------------------- Main ---------------------- //
+int main(){
+ pc.baud(9600);
+ rst1 = 0; //Set reset pin to 0
+ wait_ms(1);
+ rst1 = 1; //Set reset pin to 1
+ wait_ms(1);
+
+ xbee.attach(&xbee_recv, Serial::RxIrq);
+
+ Thread t2(thread_Motor);
+
+ while(1) {
+ Thread::wait(500); // wait 0.5s
+ }
+}
diff -r 000000000000 -r 398cc0dda9e7 mbed-rtos.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Thu Apr 28 12:01:17 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-rtos/#ee87e782d34f
diff -r 000000000000 -r 398cc0dda9e7 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Apr 28 12:01:17 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/87f2f5183dfb \ No newline at end of file