Fer Pena
/
RobotV1
aaa
Fork of RobotV1 by
main.cpp
- Committer:
- gcarmonar
- Date:
- 2015-12-17
- Revision:
- 2:aae3b2c0881c
- Parent:
- 1:1bea5dcd4d6f
- Child:
- 3:07a21d1d16e4
File content as of revision 2:aae3b2c0881c:
#include "mbed.h" #include "Servo.h" #define MAXVEL 100 // From 100 max #define SERINC 100 // 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); printf("%c\n", c); } } } void decodeInstruction(char instruction){ switch (instruction){ case 'G': case 'g': 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 'W': case 'w': 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){ float temp = 0.0; temp = (float)increment / 100.0; if (temp < 0 && SARM >= abs(temp)){ SARM = SARM + temp; }else if (temp > 0 && SARM <= 1 - abs(temp)){ SARM = SARM + temp; } } void redLed(int status){ myled = status; }