main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "M3Dpi.h"
00004 #include "jsonReporter.h"
00005 #include "binaryReporter.h"
00006 
00007 const char M3DPI_ID[] = "1234567890";
00008 
00009 M3Dpi robot;
00010 Serial pc(USBTX,USBRX);
00011 Reporter* report = new JsonReporter(&robot.xbee, M3DPI_ID);
00012 
00013 m3dpi::Distance distance;
00014 m3dpi::Direction direction;
00015 m3dpi::Rotation rotation;
00016 m3dpi::Acceleration acceleration;
00017 
00018 
00019 void setLeds(m3dpi::Distance distance)
00020 {
00021     int colors[8] = {distance.front, distance.front_right, distance.right, distance.back_right, distance.back, distance.back_left, distance.left, distance.front_left};
00022     // shift distance value to get red colors
00023     for(int i = 0; i < 8; i++){
00024         //colors[i] = ((~(colors[i]*2)) & 0xFF) << 16;
00025         if(colors[i] < 50) {colors[i] = Color::RED;}
00026         else if(colors[i] < 100) {colors[i] = Color::YELLOW;}
00027         else if(colors[i] < 150) {colors[i] = Color::GREEN;}
00028         else{colors[i] = Color::BLACK;}
00029     }
00030     robot.setLeds(colors);
00031 }
00032 
00033 void read_sensors_task(void const *name)
00034 {
00035     //Timer t; t.start();
00036     robot.setStatus(Color::BLACK);
00037     ::distance = robot.getDistance();
00038     //direction = robot.getDirection();
00039     //rotation = robot.getRotation();
00040     //acceleration = robot.getAcceleration();
00041     setLeds(::distance);
00042     robot.setStatus(Color::RED);
00043     //t.stop();pc.printf("*** Sensor acquisition time: %d us\r\n", t.read_us());
00044 }
00045 
00046 void report_task(void const *name)
00047 {
00048     //Timer t; t.start();
00049     report->distance(::distance);
00050     report->direction(direction);
00051     report->rotation(rotation);
00052     report->acceleration(acceleration);
00053     //t.stop();pc.printf("*** Report time: %d us\r\n", t.read_us());
00054 }
00055 
00056 void receive_task(void const *name)
00057 {
00058     
00059 }
00060 
00061 void idle_task(void const *name)
00062 {
00063     
00064 }
00065 
00066 int main()
00067 {            
00068     pc.baud(115200);
00069     
00070     RtosTimer read_sensors_timer(read_sensors_task, osTimerPeriodic);
00071     read_sensors_timer.start(50);
00072     
00073     RtosTimer report_timer(report_task, osTimerPeriodic);
00074 //    report_timer.start(500);
00075     
00076 //    Thread receive_thread(receive_task);
00077 //    Thread idle_thread(idle_task, NULL, osPriorityLow);
00078     
00079     Thread::wait(osWaitForever);
00080 }