Gerardo Carmona / Mbed 2 deprecated OSER-code

Firmware to control OSERobot. Check www.makerobots.tk for more details.

Dependencies:   Servo mbed

main.cpp@0:f6ff6d8ecfc0, 2016-01-06 (annotated)

Committer:
gcarmonar
Date:
Wed Jan 06 20:25:45 2016 +0000
Revision:
0:f6ff6d8ecfc0
Firmware to control OSERobot. Check www.makerobots.tk for more details.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gcarmonar 0:f6ff6d8ecfc0 1 /*
gcarmonar 0:f6ff6d8ecfc0 2 OSER - Open Source Educational Robot
gcarmonar 0:f6ff6d8ecfc0 3
gcarmonar 0:f6ff6d8ecfc0 4 Author:
gcarmonar 0:f6ff6d8ecfc0 5 Gerardo Carmona
gcarmonar 0:f6ff6d8ecfc0 6 Webpage:
gcarmonar 0:f6ff6d8ecfc0 7 www.makerobots.tk
gcarmonar 0:f6ff6d8ecfc0 8
gcarmonar 0:f6ff6d8ecfc0 9 About:
gcarmonar 0:f6ff6d8ecfc0 10 Open source and educational robot aimed to promote science and technology
gcarmonar 0:f6ff6d8ecfc0 11 over students between 15 to 17 years old.
gcarmonar 0:f6ff6d8ecfc0 12
gcarmonar 0:f6ff6d8ecfc0 13 Licence:
gcarmonar 0:f6ff6d8ecfc0 14 I provide this information so students, teachers, and all people interested
gcarmonar 0:f6ff6d8ecfc0 15 on build their own robot can get started with an easy to use robot. All
gcarmonar 0:f6ff6d8ecfc0 16 information is provided as Open Source with MIT License 2016, more details
gcarmonar 0:f6ff6d8ecfc0 17 visit https://opensource.org/licenses/MIT.
gcarmonar 0:f6ff6d8ecfc0 18
gcarmonar 0:f6ff6d8ecfc0 19 */
gcarmonar 0:f6ff6d8ecfc0 20
gcarmonar 0:f6ff6d8ecfc0 21
gcarmonar 0:f6ff6d8ecfc0 22 // --- Libraries ------------------------------------------------------------------
gcarmonar 0:f6ff6d8ecfc0 23 #include "mbed.h"
gcarmonar 0:f6ff6d8ecfc0 24 #include "Servo.h"
gcarmonar 0:f6ff6d8ecfc0 25
gcarmonar 0:f6ff6d8ecfc0 26 // --- Definitions ----------------------------------------------------------------
gcarmonar 0:f6ff6d8ecfc0 27 #define MAXVEL 100 // 100 is max
gcarmonar 0:f6ff6d8ecfc0 28 #define SERINC 100 // Move micro seconds up/down (range is 1000 to 2000)
gcarmonar 0:f6ff6d8ecfc0 29
gcarmonar 0:f6ff6d8ecfc0 30 // --- I/O pins -------------------------------------------------------------------
gcarmonar 0:f6ff6d8ecfc0 31 DigitalOut myled(LED1);
gcarmonar 0:f6ff6d8ecfc0 32 Serial pc(USBTX, USBRX);
gcarmonar 0:f6ff6d8ecfc0 33 Serial bt(PTE0, PTE1);
gcarmonar 0:f6ff6d8ecfc0 34 Servo SLF(D2); // Servo - Left Front
gcarmonar 0:f6ff6d8ecfc0 35 Servo SLB(D3); // Servo - Left Back
gcarmonar 0:f6ff6d8ecfc0 36 Servo SRF(D4); // Servo - Right Front
gcarmonar 0:f6ff6d8ecfc0 37 Servo SRB(D5); // Servo - Right Back
gcarmonar 0:f6ff6d8ecfc0 38 Servo S01(D8); // Servo Arm
gcarmonar 0:f6ff6d8ecfc0 39
gcarmonar 0:f6ff6d8ecfc0 40
gcarmonar 0:f6ff6d8ecfc0 41 // --- Prototypes -----------------------------------------------------------------
gcarmonar 0:f6ff6d8ecfc0 42 void decodeInstruction(char instruction);
gcarmonar 0:f6ff6d8ecfc0 43 void setSpeed(int left, int right);
gcarmonar 0:f6ff6d8ecfc0 44 void moveServo(int increment);
gcarmonar 0:f6ff6d8ecfc0 45
gcarmonar 0:f6ff6d8ecfc0 46 // --- Variables ------------------------------------------------------------------
gcarmonar 0:f6ff6d8ecfc0 47 char c;
gcarmonar 0:f6ff6d8ecfc0 48
gcarmonar 0:f6ff6d8ecfc0 49 // --- Main program ---------------------------------------------------------------
gcarmonar 0:f6ff6d8ecfc0 50 int main() {
gcarmonar 0:f6ff6d8ecfc0 51 // Bluetooth baud rate
gcarmonar 0:f6ff6d8ecfc0 52 bt.baud(115200);
gcarmonar 0:f6ff6d8ecfc0 53 while(1) {
gcarmonar 0:f6ff6d8ecfc0 54 // Search for data and decode instruction
gcarmonar 0:f6ff6d8ecfc0 55 if (bt.readable()){
gcarmonar 0:f6ff6d8ecfc0 56 c = bt.getc();
gcarmonar 0:f6ff6d8ecfc0 57 decodeInstruction(c);
gcarmonar 0:f6ff6d8ecfc0 58 }
gcarmonar 0:f6ff6d8ecfc0 59
gcarmonar 0:f6ff6d8ecfc0 60 }
gcarmonar 0:f6ff6d8ecfc0 61 }
gcarmonar 0:f6ff6d8ecfc0 62
gcarmonar 0:f6ff6d8ecfc0 63 // --- Functions ------------------------------------------------------------------
gcarmonar 0:f6ff6d8ecfc0 64 void decodeInstruction(char instruction){
gcarmonar 0:f6ff6d8ecfc0 65 switch (instruction){
gcarmonar 0:f6ff6d8ecfc0 66 case 'W':
gcarmonar 0:f6ff6d8ecfc0 67 case 'w':
gcarmonar 0:f6ff6d8ecfc0 68 setSpeed(MAXVEL, -MAXVEL);
gcarmonar 0:f6ff6d8ecfc0 69 break;
gcarmonar 0:f6ff6d8ecfc0 70 case 'A':
gcarmonar 0:f6ff6d8ecfc0 71 case 'a':
gcarmonar 0:f6ff6d8ecfc0 72 setSpeed(-MAXVEL, -MAXVEL);
gcarmonar 0:f6ff6d8ecfc0 73 break;
gcarmonar 0:f6ff6d8ecfc0 74 case 'S':
gcarmonar 0:f6ff6d8ecfc0 75 case 's':
gcarmonar 0:f6ff6d8ecfc0 76 setSpeed(-MAXVEL, MAXVEL);
gcarmonar 0:f6ff6d8ecfc0 77 break;
gcarmonar 0:f6ff6d8ecfc0 78 case 'D':
gcarmonar 0:f6ff6d8ecfc0 79 case 'd':
gcarmonar 0:f6ff6d8ecfc0 80 setSpeed(+MAXVEL, +MAXVEL);
gcarmonar 0:f6ff6d8ecfc0 81 break;
gcarmonar 0:f6ff6d8ecfc0 82 case 'Q':
gcarmonar 0:f6ff6d8ecfc0 83 case 'q':
gcarmonar 0:f6ff6d8ecfc0 84 setSpeed(0,0);
gcarmonar 0:f6ff6d8ecfc0 85 break;
gcarmonar 0:f6ff6d8ecfc0 86 case 'P':
gcarmonar 0:f6ff6d8ecfc0 87 case 'p':
gcarmonar 0:f6ff6d8ecfc0 88 moveServo(SERINC);
gcarmonar 0:f6ff6d8ecfc0 89 break;
gcarmonar 0:f6ff6d8ecfc0 90 case 'L':
gcarmonar 0:f6ff6d8ecfc0 91 case 'l':
gcarmonar 0:f6ff6d8ecfc0 92 moveServo(-SERINC);
gcarmonar 0:f6ff6d8ecfc0 93 break;
gcarmonar 0:f6ff6d8ecfc0 94 }
gcarmonar 0:f6ff6d8ecfc0 95 }
gcarmonar 0:f6ff6d8ecfc0 96
gcarmonar 0:f6ff6d8ecfc0 97 void setSpeed(int left, int right){
gcarmonar 0:f6ff6d8ecfc0 98 float sLeft=0, sRight=0;
gcarmonar 0:f6ff6d8ecfc0 99 sLeft = (float)(left + 100)/200;
gcarmonar 0:f6ff6d8ecfc0 100 sRight = (float)(right + 100)/200;
gcarmonar 0:f6ff6d8ecfc0 101 SLF = sLeft;
gcarmonar 0:f6ff6d8ecfc0 102 SLB = sLeft;
gcarmonar 0:f6ff6d8ecfc0 103 SRF = sRight;
gcarmonar 0:f6ff6d8ecfc0 104 SRB = sRight;
gcarmonar 0:f6ff6d8ecfc0 105 }
gcarmonar 0:f6ff6d8ecfc0 106
gcarmonar 0:f6ff6d8ecfc0 107 void moveServo(int increment){
gcarmonar 0:f6ff6d8ecfc0 108 float temp = 0.0;
gcarmonar 0:f6ff6d8ecfc0 109 float pos;
gcarmonar 0:f6ff6d8ecfc0 110 temp = (float)increment / 100.0;
gcarmonar 0:f6ff6d8ecfc0 111 pos = S01;
gcarmonar 0:f6ff6d8ecfc0 112 if (temp < 0 && pos >= abs(temp)){
gcarmonar 0:f6ff6d8ecfc0 113 S01 = pos + temp;
gcarmonar 0:f6ff6d8ecfc0 114 }else if (temp > 0 && pos <= 1 - abs(temp)){
gcarmonar 0:f6ff6d8ecfc0 115 S01 = pos + temp;
gcarmonar 0:f6ff6d8ecfc0 116 }
gcarmonar 0:f6ff6d8ecfc0 117 }