Diff: main.cpp

Revision:

0:316cfb89e9cd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Apr 29 20:02:35 2020 +0000
@@ -0,0 +1,162 @@
+// full-speed reverse (-1.0)
+// full speed forwards (1.0)
+
+#include "mbed.h"
+#include "motordriver.h"
+#include "Servo.h"
+
+Motor left(p22, p12, p11, 1); // pwm, fwd, rev (Motor serialChar)
+Motor right(p21, p10, p9, 1); // pwm, fwd, rev (Motor B)
+Servo shoulder(p23);
+Servo elbow(p24);
+Servo wrist(p26);
+Servo wrist(p25);
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+
+//set up xbee
+Serial xbee(p13, p14);//TX not used
+Serial pc(USBTX, USBRX);
+DigitalOut rst(p7);
+
+//speed variables for motors
+volatile float speed1;
+volatile float speed2;
+
+void go(){
+    left.speed(speed1);
+    right.speed(speed2);    
+}
+
+int main() {
+    
+    // reset xbee (at least 200ns)
+    rst = 0;
+    wait_ms(1);
+    rst = 1;
+    wait_ms(1); 
+    
+    //initial speed is 0
+    speed1 = 0.0;
+    speed2 = 0.0;
+    
+    //variables to hold serial characters for xbee
+    char serialChar;
+    char prev;
+    
+    //varaibles for arm servos
+    volatile  float shoulderNum = 0.5;
+    volatile  float elbowNum = 0.10;
+    volatile  float wristNum = 0.0;
+    
+    //loop
+    while (1) {
+
+        if(xbee.readable()){
+            prev = serialChar;
+            serialChar = xbee.getc();
+            
+            
+            //left right rotation
+            if (prev == 'a' && serialChar < 60){
+                shoulderNum = shoulderNum +  0.01;
+                if (shoulderNum > 1)
+                    shoulderNum = 1;
+            shoulder = shoulderNum;
+            led1 = 1;
+            }
+            else if (prev == 'a' && serialChar > 110){
+                shoulderNum = shoulderNum - 0.01;
+                if (shoulderNum < 0)
+                    shoulderNum = 0;
+            shoulder = shoulderNum;
+            led2 = 1;
+            }
+            else{
+                shoulder = shoulderNum;
+            }
+            
+            
+            //up down rotation
+            if (prev == 'b' && serialChar < 60){
+                elbowNum = elbowNum +  0.01;
+                if (elbowNum > 1)
+                    elbowNum = 1;
+                elbow = elbowNum;
+                led1 = 1;
+            }
+            else if (prev == 'b' && serialChar > 110){
+                elbowNum = elbowNum - 0.01;
+                if (elbowNum < 0)
+                    elbowNum = 0;
+                elbow = elbowNum;
+                led2 = 1;
+            }
+            else{
+                elbow = elbowNum;
+            }
+            
+            //motor rotation
+            if (prev == 'c' && serialChar < 60){
+                speed1 = 1.0;
+                speed2 = -1.0;
+                go();
+            }
+            else if (prev == 'c' && serialChar > 110){
+                speed1 = -1;
+                speed2 = 1;
+                go();
+            }
+            else if (prev == 'c' && serialChar > 60 && serialChar < 110 && speed1 != speed2){
+                speed1 = 0;
+                speed2 = 0;
+                go();
+            }
+            
+            //motor forward and reverse
+            if (prev == 'd' && serialChar < 60){
+                speed1 = -1.0;
+                speed2 = -1.0;
+                go();
+            }
+            else if (prev == 'd' && serialChar > 110){
+                speed1 = 1;
+                speed2 = 1;
+                go();
+            }
+            else if (prev == 'd' && serialChar > 60 && serialChar < 110 && speed1 == speed2){
+                speed1 = 0;
+                speed2 = 0;
+                go();
+            }
+            
+            
+            
+            
+            //wrist controller
+            if (prev == 'e' && serialChar == 36){
+                wristNum = wristNum +  0.01;
+                if (wristNum > 1)
+                    wristNum = 1;
+                wrist = wristNum;
+                led1 = 1;
+            }
+            else if (prev == 'f' && serialChar == 36){
+                wristNum = wristNum - 0.01;
+                if (wristNum < 0)
+                    wristNum = 0;
+                wrist = wristNum;
+                led2 = 1;
+            }
+            else{
+                wrist = wristNum;
+            }
+            
+            //print to pc for debugging (Tera Term etc.)
+            pc.putc(serialChar);
+        }
+    }
+}