Diff: main.cpp

Revision:

0:b52adb14e1a9

Child:

1:3f064275f04d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Apr 25 18:35:31 2020 +0000
@@ -0,0 +1,119 @@
+#include "mbed.h"#include "mbed.h"
+#include "Servo.h"
+#include "Motor.h"
+#include "rtos.h"
+
+Serial pi(USBTX, USBRX);
+DigitalIn leftEncoder(p22);
+DigitalIn rightEncoder(p21);
+Motor leftMotor(p23, p30, p29);
+Motor rightMotor(p24, p5, p6);
+Servo topServo(p25);
+Servo bottomServo(p26);
+
+
+volatile float topServoPos = 0.0;
+volatile float bottomServoPos = 0.0;
+volatile int xpadState = 0;
+volatile int ypadState = 0;
+volatile float leftJoystickState = 0.0;
+volatile float rightJoystickState = 0.0;
+
+//rotate the pan/tilt servos
+void RotateServos()
+{
+    if(xpadState == 1)
+    {
+        bottomServoPos = bottomServoPos + 0.05;
+        if(bottomServoPos > 1.0) bottomServoPos = 1.0;
+    }
+    else if(xpadState == -1)
+    {
+        bottomServoPos = bottomServoPos - 0.05;
+        if(bottomServoPos < 0.0) bottomServoPos = 0.0;
+    }
+    
+    if(ypadState == 1)
+    {
+        topServoPos = topServoPos + 0.05;
+        if(topServoPos > 1.0) topServoPos = 1.0;
+    }
+    else if(xpadState == -1)
+    {
+        topServoPos = topServoPos - 0.05;
+        if(topServoPos < 0.0) topServoPos = 0.0;
+    }
+    
+    topServo = topServoPos;
+    bottomServo = bottomServoPos;
+    Thread::wait(150);
+}
+
+
+//Sets and keeps motor speed matched using encoder output
+void SpeedBalance()
+{
+    leftMotor.speed(leftJoystickState);
+    rightMotor.speed(rightJoystickState);
+    if(leftJoystickState != 0 && rightJoystickState != 0 && leftJoystickState == rightJoystickState) //only need to match speeds if they're moving in same direction
+    {
+        
+    }
+    Thread::yield();
+}
+
+int main() 
+{
+    Thread motorControl(SpeedBalance);
+    Thread servoControl(RotateServos);
+    
+    char servoNum = '0';
+    char leftMotNum = '0';
+    char rightMotNum = '0';
+    
+    while(1) 
+    {
+        if(pi.readable()) //check if new command from pi available
+        {
+            if (pi.getc() == '!' ) {
+                servoNum = pi.getc();
+                if (servoNum == '1') {
+                    xpadState = -1;
+                }
+                if (servoNum == '2') {
+                    xpadState = 1;
+                }
+                if (servoNum == '3') {
+                    ypadState = 1;
+                }
+                if (servoNum == '4') {
+                    ypadState = -1;
+                }
+            }
+            else if (pi.getc() == '(') {
+                leftMotNum = pi.getc();
+                if (leftMotNum == '0') {
+                    leftJoystickState = 0;
+                }
+                if (leftMotNum == '1') {
+                    leftJoystickState = 1;
+                }
+                if (leftMotNum == '2') {
+                    leftJoystickState = -1;
+                }
+            }
+            else if (pi.getc() == ')' ) {
+                rightMotNum = pi.getc();
+                if (rightMotNum == '0') {
+                    rightJoystickState = 0;
+                }
+                if (rightMotNum == '1') {
+                    rightJoystickState = 1;
+                }
+                if (rightMotNum == '2') {
+                    rightJoystickState = -1;
+                }
+            }
+        }
+    }
+}