CSCI 5360 JB_MB_BC
/
m3pi_reciever
Executes commands received via xbee.
main.cpp
- Committer:
- mb4899
- Date:
- 2015-05-03
- Revision:
- 5:854aea46d7b8
- Parent:
- 4:c7a00aca4520
File content as of revision 5:854aea46d7b8:
#include "mbed.h" #include "m3pi.h" #include "Protocol.h" Serial xbee(p28, p27); m3pi m3pi; void set_command(); void execute(); int commands[3]; int main() { m3pi.cls(); // Set initial "default" commands for m3pi // Action (stop), Direction (left), and Duration (zero), respectively commands[0] = 6; commands[1] = 0; commands[2] = 0; while (1) { if (xbee.readable()) { xbee.scanf("|%i,%i,%i@", &commands[0], &commands[1], &commands[2]); } m3pi.locate(0,1); m3pi.printf("%i,%i,%i", commands[0], commands[1], commands[2]); execute(); } } void execute() { float speed = 0.25; // Default initial speed float turn_speed = 0.125; // Default turn speed // For ease in switch block readability int action = commands[0]; int direction = commands[1]; int duration = commands[2]; switch (action) { case MOVE: if (direction == FORWARD) { m3pi.forward(speed); } else if (direction == LEFT) { m3pi.left(turn_speed); m3pi.forward(speed); } else if (direction == RIGHT) { m3pi.right(turn_speed); m3pi.forward(speed); } else if (direction == BACKWARD) { m3pi.backward(speed); } break; case TURN: if (direction == LEFT) { m3pi.left(turn_speed); } else if (direction == RIGHT) { m3pi.right(turn_speed); } break; case RUN: if (direction == FORWARD) { speed *= 2; m3pi.forward(speed); } else if (direction == LEFT) { m3pi.left(turn_speed); speed *= 2; m3pi.forward(speed); } else if (direction == RIGHT) { m3pi.right(turn_speed); speed *= 2; m3pi.forward(speed); } else if (direction == BACKWARD) { speed *= 2; m3pi.backward(speed); } break; case STOP: m3pi.stop(); break; } wait(duration); m3pi.stop(); commands[0] = 6; commands[1] = 0; commands[2] = 0; // Reset "defaults" }