Sille Van Landschoot / Mbed 2 deprecated m3Dpi-helloworld

m3Dpi robot, based on the Pololu 3pi and m3pi. m3Dpi has multiple distance sensors, gyroscope, compass and accelerometer sensor to be fully aware of its environment. With the addition of xbee or nrf24n01 module it has wireless communication capabilities.

Dependencies:   m3Dpi mbed-rtos mbed MbedJSONValue

main.cpp@10:a5f43326e0b0, 2015-12-17 (annotated)

Committer:
kudrom
Date:
Thu Dec 17 14:51:19 2015 +0000
Revision:
10:a5f43326e0b0
Parent:
9:5292bf545459 
Last changes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sillevl 7:9068fc754a0b 1 #include "mbed.h"
sillevl 7:9068fc754a0b 2 #include "rtos.h"
sillevl 1:6f620263c9f8 3 #include "M3Dpi.h"
sillevl 7:9068fc754a0b 4 #include "jsonReporter.h"
sillevl 6:414e1fea64f3 5
kudrom 8:5c0833506d67 6 const char M3DPI_ID[] = "first";
sillevl 6:414e1fea64f3 7
sillevl 7:9068fc754a0b 8 M3Dpi robot;
sillevl 7:9068fc754a0b 9 Serial pc(USBTX,USBRX);
kudrom 9:5292bf545459 10 JsonReporter report(&robot.xbee, M3DPI_ID);
sillevl 6:414e1fea64f3 11
kudrom 8:5c0833506d67 12 static float r_wheel = 0.0;
kudrom 8:5c0833506d67 13 static float l_wheel = 0.0;
kudrom 8:5c0833506d67 14 char direction = 'f';
kudrom 8:5c0833506d67 15
sillevl 6:414e1fea64f3 16
sillevl 6:414e1fea64f3 17 void setLeds(m3dpi::Distance distance)
sillevl 6:414e1fea64f3 18 {
sillevl 6:414e1fea64f3 19 int colors[8] = {distance.front, distance.front_right, distance.right, distance.back_right, distance.back, distance.back_left, distance.left, distance.front_left};
sillevl 6:414e1fea64f3 20 // shift distance value to get red colors
sillevl 6:414e1fea64f3 21 for(int i = 0; i < 8; i++){
sillevl 6:414e1fea64f3 22 colors[i] = colors[i] << 16;
sillevl 6:414e1fea64f3 23 }
sillevl 6:414e1fea64f3 24 robot.setLeds(colors);
sillevl 6:414e1fea64f3 25 }
sillevl 6:414e1fea64f3 26
sillevl 7:9068fc754a0b 27 void read_sensors_thread(void const * args)
sillevl 6:414e1fea64f3 28 {
sillevl 7:9068fc754a0b 29 robot.setStatus(Color::GREEN);
sillevl 6:414e1fea64f3 30 m3dpi::Distance distance = robot.getDistance();
sillevl 7:9068fc754a0b 31 setLeds(distance);
sillevl 6:414e1fea64f3 32
sillevl 7:9068fc754a0b 33 report.distance(distance);
sillevl 7:9068fc754a0b 34 report.direction(robot.getDirection());
sillevl 7:9068fc754a0b 35 report.rotation(robot.getRotation());
sillevl 7:9068fc754a0b 36 report.acceleration(robot.getAcceleration());
sillevl 6:414e1fea64f3 37
sillevl 7:9068fc754a0b 38 report.time(robot.getTime());
kudrom 8:5c0833506d67 39 robot.setStatus(Color::BLACK);
kudrom 8:5c0833506d67 40 }
kudrom 8:5c0833506d67 41
kudrom 8:5c0833506d67 42 void read_commands_thread(void const *args)
kudrom 8:5c0833506d67 43 {
kudrom 10:a5f43326e0b0 44 while(robot.xbee.readable()){
kudrom 9:5292bf545459 45 char command = robot.xbee.getc();
kudrom 8:5c0833506d67 46 switch(command){
kudrom 8:5c0833506d67 47 case 'l':
kudrom 8:5c0833506d67 48 // Left
kudrom 10:a5f43326e0b0 49 l_wheel = -0.10;
kudrom 10:a5f43326e0b0 50 r_wheel = 0.10;
kudrom 8:5c0833506d67 51 break;
kudrom 8:5c0833506d67 52 case 'r':
kudrom 8:5c0833506d67 53 // Right
kudrom 10:a5f43326e0b0 54 l_wheel = 0.10;
kudrom 10:a5f43326e0b0 55 r_wheel = -0.10;
kudrom 8:5c0833506d67 56 break;
kudrom 8:5c0833506d67 57 case 'f':
kudrom 8:5c0833506d67 58 // Frontward
kudrom 10:a5f43326e0b0 59 l_wheel = 0.1;
kudrom 10:a5f43326e0b0 60 r_wheel = 0.1;
kudrom 8:5c0833506d67 61 break;
kudrom 8:5c0833506d67 62 case 'b':
kudrom 8:5c0833506d67 63 // Backward
kudrom 10:a5f43326e0b0 64 l_wheel = -0.1;
kudrom 10:a5f43326e0b0 65 r_wheel = -0.1;
kudrom 8:5c0833506d67 66 break;
kudrom 8:5c0833506d67 67 case 's':
kudrom 8:5c0833506d67 68 // Brake
kudrom 10:a5f43326e0b0 69 l_wheel = 0;
kudrom 8:5c0833506d67 70 r_wheel = 0;
kudrom 8:5c0833506d67 71 break;
kudrom 8:5c0833506d67 72 default:
kudrom 10:a5f43326e0b0 73 robot.xbee.printf("Unknown command %c.\n", command);
kudrom 8:5c0833506d67 74 }
kudrom 10:a5f43326e0b0 75 robot.left_motor(l_wheel);
kudrom 10:a5f43326e0b0 76 robot.right_motor(r_wheel);
kudrom 8:5c0833506d67 77 }
kudrom 10:a5f43326e0b0 78
kudrom 10:a5f43326e0b0 79 // Stop if the robot doesn't receive anything in 100 ms.
kudrom 10:a5f43326e0b0 80 l_wheel = 0;
kudrom 10:a5f43326e0b0 81 r_wheel = 0;
sillevl 6:414e1fea64f3 82 }
sillevl 0:ab0ce9ab81bf 83
sillevl 1:6f620263c9f8 84 int main()
sillevl 6:414e1fea64f3 85 {
sillevl 1:6f620263c9f8 86 pc.baud(115200);
sillevl 6:414e1fea64f3 87
kudrom 8:5c0833506d67 88 RtosTimer readSensorsThread(read_sensors_thread, osTimerPeriodic);
kudrom 8:5c0833506d67 89 readSensorsThread.start(1000);
kudrom 8:5c0833506d67 90
kudrom 10:a5f43326e0b0 91 Thread readCommandsThread(read_commands_thread);
kudrom 10:a5f43326e0b0 92 //readCommandsThread.start(100);
sillevl 0:ab0ce9ab81bf 93
sillevl 7:9068fc754a0b 94 Thread::wait(osWaitForever);
sillevl 0:ab0ce9ab81bf 95 }