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?
User | Revision | Line number | New 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 | } |