Gerardo Carmona / Mbed 2 deprecated RobotV1

Robot Competition V1

Dependencies:   Servo mbed

main.cpp@2:aae3b2c0881c, 2015-12-17 (annotated)

Committer:
gcarmonar
Date:
Thu Dec 17 18:24:42 2015 +0000
Revision:
2:aae3b2c0881c
Parent:
1:1bea5dcd4d6f 
correciones d ebugs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gcarmonar 0:4a46db9b1f3e 1 #include "mbed.h"
gcarmonar 0:4a46db9b1f3e 2 #include "Servo.h"
gcarmonar 0:4a46db9b1f3e 3
gcarmonar 0:4a46db9b1f3e 4 #define MAXVEL 100 // From 100 max
gcarmonar 2:aae3b2c0881c 5 #define SERINC 100 // Move micro seconds up/down
gcarmonar 0:4a46db9b1f3e 6
gcarmonar 0:4a46db9b1f3e 7
gcarmonar 0:4a46db9b1f3e 8 DigitalOut myled(LED1);
gcarmonar 0:4a46db9b1f3e 9 Serial pc(USBTX, USBRX);
gcarmonar 0:4a46db9b1f3e 10 Serial bt(PTE0, PTE1);
gcarmonar 0:4a46db9b1f3e 11 Servo SLF(D2); // Servo Left Front
gcarmonar 0:4a46db9b1f3e 12 Servo SLB(D3); // Servo Left Back
gcarmonar 0:4a46db9b1f3e 13 Servo SRF(D4); // Servo Right Front
gcarmonar 0:4a46db9b1f3e 14 Servo SRB(D5); // Servo Right Back
gcarmonar 0:4a46db9b1f3e 15 Servo SARM(D8); // Servo Arm
gcarmonar 0:4a46db9b1f3e 16
gcarmonar 0:4a46db9b1f3e 17
gcarmonar 0:4a46db9b1f3e 18 void decodeInstruction(char instruction);
gcarmonar 0:4a46db9b1f3e 19 void setSpeed(int left, int right);
gcarmonar 0:4a46db9b1f3e 20 void moveServo(int increment);
gcarmonar 0:4a46db9b1f3e 21 void redLed(int status);
gcarmonar 0:4a46db9b1f3e 22
gcarmonar 0:4a46db9b1f3e 23 char c;
gcarmonar 0:4a46db9b1f3e 24
gcarmonar 0:4a46db9b1f3e 25
gcarmonar 0:4a46db9b1f3e 26 int main() {
gcarmonar 0:4a46db9b1f3e 27 bt.baud(115200);
gcarmonar 0:4a46db9b1f3e 28 while(1) {
gcarmonar 0:4a46db9b1f3e 29 if (bt.readable()){
gcarmonar 0:4a46db9b1f3e 30 c = bt.getc();
gcarmonar 0:4a46db9b1f3e 31 pc.printf("Received: %c\n\r", c);
gcarmonar 0:4a46db9b1f3e 32 decodeInstruction(c);
gcarmonar 2:aae3b2c0881c 33 printf("%c\n", c);
gcarmonar 0:4a46db9b1f3e 34 }
gcarmonar 0:4a46db9b1f3e 35
gcarmonar 0:4a46db9b1f3e 36 }
gcarmonar 0:4a46db9b1f3e 37 }
gcarmonar 0:4a46db9b1f3e 38
gcarmonar 0:4a46db9b1f3e 39
gcarmonar 0:4a46db9b1f3e 40 void decodeInstruction(char instruction){
gcarmonar 0:4a46db9b1f3e 41 switch (instruction){
gcarmonar 1:1bea5dcd4d6f 42 case 'G':
gcarmonar 1:1bea5dcd4d6f 43 case 'g':
gcarmonar 1:1bea5dcd4d6f 44 setSpeed(MAXVEL, -MAXVEL);
gcarmonar 0:4a46db9b1f3e 45 break;
gcarmonar 0:4a46db9b1f3e 46 case 'A':
gcarmonar 0:4a46db9b1f3e 47 case 'a':
gcarmonar 1:1bea5dcd4d6f 48 setSpeed(-MAXVEL, -MAXVEL);
gcarmonar 0:4a46db9b1f3e 49 break;
gcarmonar 0:4a46db9b1f3e 50 case 'S':
gcarmonar 0:4a46db9b1f3e 51 case 's':
gcarmonar 1:1bea5dcd4d6f 52 setSpeed(-MAXVEL, MAXVEL);
gcarmonar 0:4a46db9b1f3e 53 break;
gcarmonar 0:4a46db9b1f3e 54 case 'D':
gcarmonar 0:4a46db9b1f3e 55 case 'd':
gcarmonar 1:1bea5dcd4d6f 56 setSpeed(+MAXVEL, +MAXVEL);
gcarmonar 0:4a46db9b1f3e 57 break;
gcarmonar 0:4a46db9b1f3e 58 case 'Q':
gcarmonar 0:4a46db9b1f3e 59 case 'q':
gcarmonar 0:4a46db9b1f3e 60 setSpeed(0,0);
gcarmonar 0:4a46db9b1f3e 61 break;
gcarmonar 0:4a46db9b1f3e 62 case 'P':
gcarmonar 0:4a46db9b1f3e 63 case 'p':
gcarmonar 0:4a46db9b1f3e 64 moveServo(SERINC);
gcarmonar 0:4a46db9b1f3e 65 break;
gcarmonar 0:4a46db9b1f3e 66 case 'L':
gcarmonar 0:4a46db9b1f3e 67 case 'l':
gcarmonar 0:4a46db9b1f3e 68 moveServo(-SERINC);
gcarmonar 0:4a46db9b1f3e 69 break;
gcarmonar 1:1bea5dcd4d6f 70 case 'W':
gcarmonar 1:1bea5dcd4d6f 71 case 'w':
gcarmonar 0:4a46db9b1f3e 72 redLed(1);
gcarmonar 0:4a46db9b1f3e 73 break;
gcarmonar 0:4a46db9b1f3e 74 case 'H':
gcarmonar 0:4a46db9b1f3e 75 case 'h':
gcarmonar 0:4a46db9b1f3e 76 redLed(0);
gcarmonar 0:4a46db9b1f3e 77 break;
gcarmonar 0:4a46db9b1f3e 78 }
gcarmonar 0:4a46db9b1f3e 79 }
gcarmonar 0:4a46db9b1f3e 80
gcarmonar 0:4a46db9b1f3e 81 void setSpeed(int left, int right){
gcarmonar 1:1bea5dcd4d6f 82 float sLeft=0, sRight=0;
gcarmonar 0:4a46db9b1f3e 83 sLeft = (float)(left + 100)/200;
gcarmonar 1:1bea5dcd4d6f 84 sRight = (float)(right + 100)/200;
gcarmonar 0:4a46db9b1f3e 85 SLF = sLeft;
gcarmonar 0:4a46db9b1f3e 86 SLB = sLeft;
gcarmonar 0:4a46db9b1f3e 87 SRF = sRight;
gcarmonar 0:4a46db9b1f3e 88 SRB = sRight;
gcarmonar 0:4a46db9b1f3e 89 }
gcarmonar 0:4a46db9b1f3e 90
gcarmonar 0:4a46db9b1f3e 91 void moveServo(int increment){
gcarmonar 2:aae3b2c0881c 92 float temp = 0.0;
gcarmonar 2:aae3b2c0881c 93 temp = (float)increment / 100.0;
gcarmonar 2:aae3b2c0881c 94 if (temp < 0 && SARM >= abs(temp)){
gcarmonar 2:aae3b2c0881c 95 SARM = SARM + temp;
gcarmonar 2:aae3b2c0881c 96 }else if (temp > 0 && SARM <= 1 - abs(temp)){
gcarmonar 2:aae3b2c0881c 97 SARM = SARM + temp;
gcarmonar 0:4a46db9b1f3e 98 }
gcarmonar 0:4a46db9b1f3e 99 }
gcarmonar 0:4a46db9b1f3e 100
gcarmonar 0:4a46db9b1f3e 101 void redLed(int status){
gcarmonar 0:4a46db9b1f3e 102 myled = status;
gcarmonar 0:4a46db9b1f3e 103 }