Diff: main.cpp

Revision:

0:f6ff6d8ecfc0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jan 06 20:25:45 2016 +0000
@@ -0,0 +1,117 @@
+/*
+    OSER - Open Source Educational Robot
+
+    Author: 
+        Gerardo Carmona
+    Webpage: 
+        www.makerobots.tk
+
+    About:
+        Open source and educational robot aimed to promote science and technology
+        over students between 15 to 17 years old.
+
+    Licence:
+        I provide this information so students, teachers, and all people interested 
+        on build their own robot can get started with an easy to use robot. All 
+        information is provided as Open Source with MIT License 2016, more details 
+        visit https://opensource.org/licenses/MIT.
+
+*/
+
+
+// --- Libraries ------------------------------------------------------------------
+#include "mbed.h"
+#include "Servo.h"
+
+// --- Definitions ----------------------------------------------------------------
+#define MAXVEL      100   // 100 is max
+#define SERINC      100   // Move micro seconds up/down (range is 1000 to 2000)
+
+// --- I/O pins -------------------------------------------------------------------
+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 S01(D8);           // Servo Arm
+
+
+// --- Prototypes -----------------------------------------------------------------
+void decodeInstruction(char instruction);
+void setSpeed(int left, int right);
+void moveServo(int increment);
+
+// --- Variables ------------------------------------------------------------------
+char c;
+
+// --- Main program ---------------------------------------------------------------
+int main() {
+    // Bluetooth baud rate
+    bt.baud(115200);
+    while(1) {
+        // Search for data and decode instruction
+        if (bt.readable()){
+            c = bt.getc();
+            decodeInstruction(c);
+        }
+            
+    }
+}
+
+// --- Functions ------------------------------------------------------------------
+void decodeInstruction(char instruction){
+    switch (instruction){
+        case 'W':
+        case 'w':
+            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;
+    }        
+}
+
+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;
+    float pos;
+    temp = (float)increment / 100.0;
+    pos = S01;
+    if (temp < 0 && pos >= abs(temp)){
+        S01 = pos + temp;
+    }else if (temp > 0 && pos <= 1 - abs(temp)){
+        S01 = pos + temp;
+    }
+}
\ No newline at end of file