Fer Pena
/
RobotV1
aaa
Fork of RobotV1 by
main.cpp
- Committer:
- gcarmonar
- Date:
- 2015-12-01
- Revision:
- 0:4a46db9b1f3e
- Child:
- 1:1bea5dcd4d6f
File content as of revision 0:4a46db9b1f3e:
#include "mbed.h" #include "Servo.h" #define MAXVEL 100 // From 100 max #define SERINC 0.1 // Move micro seconds up/down DigitalOut myled(LED1); Serial pc(USBTX, USBRX); Serial bt(PTE0, PTE1); Servo SLF(D2); // Servo Left Front Servo SLB(D3); // Servo Left Back Servo SRF(D4); // Servo Right Front Servo SRB(D5); // Servo Right Back Servo SARM(D8); // Servo Arm void decodeInstruction(char instruction); void setSpeed(int left, int right); void moveServo(int increment); void redLed(int status); char c; int main() { bt.baud(115200); while(1) { if (bt.readable()){ c = bt.getc(); pc.printf("Received: %c\n\r", c); decodeInstruction(c); } } } void decodeInstruction(char instruction){ switch (instruction){ case 'F': case 'f': setSpeed(MAXVEL, MAXVEL); break; case 'A': case 'a': setSpeed(-MAXVEL, MAXVEL); break; case 'S': case 's': setSpeed(-MAXVEL, -MAXVEL); break; case 'D': case 'd': setSpeed(MAXVEL, -MAXVEL); break; case 'Q': case 'q': setSpeed(0,0); break; case 'P': case 'p': moveServo(SERINC); break; case 'L': case 'l': moveServo(-SERINC); break; case 'G': case 'g': redLed(1); break; case 'H': case 'h': redLed(0); break; } } void setSpeed(int left, int right){ float sleft=0, sright=0; sLeft = (float)(left + 100)/200; sright = (float)(right + 100)/200; SLF = sLeft; SLB = sLeft; SRF = sRight; SRB = sRight; } void moveServo(int increment){ if (increment < 0 && SARM >= abs(increment){ SARM = SARM + increment; elseif (increment > 0 && SARM <= 1 - abs(increment){ SARM = SARM + increment; } } void redLed(int status){ myled = status; }