Sille Van Landschoot / Mbed 2 deprecated m3Dpi-helloworld

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   m3Dpi mbed-rtos mbed MbedJSONValue

main.cpp@9:5292bf545459, 2015-12-16 (annotated)

Committer:
kudrom
Date:
Wed Dec 16 13:27:43 2015 +0000
Revision:
9:5292bf545459
Parent:
8:5c0833506d67
Child:
10:a5f43326e0b0
xbee

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 9:5292bf545459 44 if(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 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 }