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@8:5c0833506d67, 2015-12-11 (annotated)

Committer:
kudrom
Date:
Fri Dec 11 10:40:40 2015 +0000
Revision:
8:5c0833506d67
Parent:
7:9068fc754a0b
Child:
9:5292bf545459
Added battery to the reporter and the commands interface.

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);
sillevl 7:9068fc754a0b 10 JsonReporter report(&pc, 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 8:5c0833506d67 44 if(pc.readable()){
kudrom 8:5c0833506d67 45 char command = pc.getc();
kudrom 8:5c0833506d67 46 switch(command){
kudrom 8:5c0833506d67 47 case 'l':
kudrom 8:5c0833506d67 48 // Left
kudrom 8:5c0833506d67 49 if(direction == 'f'){
kudrom 8:5c0833506d67 50 l_wheel += 0.1;
kudrom 8:5c0833506d67 51 }else{
kudrom 8:5c0833506d67 52 l_wheel -= 0.1;
kudrom 8:5c0833506d67 53 }
kudrom 8:5c0833506d67 54 break;
kudrom 8:5c0833506d67 55 case 'r':
kudrom 8:5c0833506d67 56 // Right
kudrom 8:5c0833506d67 57 if(direction == 'f'){
kudrom 8:5c0833506d67 58 r_wheel += 0.1;
kudrom 8:5c0833506d67 59 }else{
kudrom 8:5c0833506d67 60 r_wheel -= 0.1;
kudrom 8:5c0833506d67 61 }
kudrom 8:5c0833506d67 62 break;
kudrom 8:5c0833506d67 63 case 'f':
kudrom 8:5c0833506d67 64 // Frontward
kudrom 8:5c0833506d67 65 r_wheel += 0.1;
kudrom 8:5c0833506d67 66 l_wheel += 0.1;
kudrom 8:5c0833506d67 67 direction = 'f';
kudrom 8:5c0833506d67 68 break;
kudrom 8:5c0833506d67 69 case 'b':
kudrom 8:5c0833506d67 70 // Backward
kudrom 8:5c0833506d67 71 r_wheel -= 0.1;
kudrom 8:5c0833506d67 72 l_wheel -= 0.1;
kudrom 8:5c0833506d67 73 direction = 'b';
kudrom 8:5c0833506d67 74 break;
kudrom 8:5c0833506d67 75 case 's':
kudrom 8:5c0833506d67 76 // Brake
kudrom 8:5c0833506d67 77 r_wheel = 0;
kudrom 8:5c0833506d67 78 l_wheel = 0;
kudrom 8:5c0833506d67 79 direction = 'f';
kudrom 8:5c0833506d67 80 break;
kudrom 8:5c0833506d67 81 default:
kudrom 8:5c0833506d67 82 pc.printf("Unknown command %c.\n", command);
kudrom 8:5c0833506d67 83 }
kudrom 8:5c0833506d67 84
kudrom 8:5c0833506d67 85 robot.left_motor(l_wheel);
kudrom 8:5c0833506d67 86 robot.right_motor(r_wheel);
kudrom 8:5c0833506d67 87 }
sillevl 6:414e1fea64f3 88 }
sillevl 0:ab0ce9ab81bf 89
sillevl 1:6f620263c9f8 90 int main()
sillevl 6:414e1fea64f3 91 {
sillevl 1:6f620263c9f8 92 pc.baud(115200);
sillevl 6:414e1fea64f3 93
kudrom 8:5c0833506d67 94 RtosTimer readSensorsThread(read_sensors_thread, osTimerPeriodic);
kudrom 8:5c0833506d67 95 readSensorsThread.start(1000);
kudrom 8:5c0833506d67 96
kudrom 8:5c0833506d67 97 RtosTimer readCommandsThread(read_commands_thread, osTimerPeriodic);
kudrom 8:5c0833506d67 98 readCommandsThread.start(50);
sillevl 0:ab0ce9ab81bf 99
sillevl 7:9068fc754a0b 100 Thread::wait(osWaitForever);
sillevl 0:ab0ce9ab81bf 101 }