Diff: main.cpp

Revision:

4:22910b683dea

Parent:

2:3d1a5025c32a

Child:

6:0597a4ac4696

--- a/main.cpp	Mon Apr 27 01:25:01 2020 +0000
+++ b/main.cpp	Mon Apr 27 02:44:10 2020 +0000
@@ -11,24 +11,20 @@
 Servo topServo(p25);
 Servo bottomServo(p26);
 
-
-volatile float topServoPos = 0.0;
-volatile float bottomServoPos = 0.0;
-volatile float leftMotorSpeed = 0.0;
-volatile float rightMotorSpeed = 0.0;
 volatile int xpadState = 0; //left-right Dpad
 volatile int ypadState = 0; //up-down Dpad
 volatile int leftJoystickState = 0;
 volatile int rightJoystickState = 0;
-volatile int leftEncoderCount = 0;
+volatile int leftEncoderCount = 0; //keep track of encoder ticks
 volatile int rightEncoderCount = 0;
-volatile bool interruptAttached = false;
 
+//increment left encoder counts
 void LeftCount()
 {
     leftEncoderCount = leftEncoderCount + 1;
 }
 
+//increment right encoder counts
 void RightCount()
 {
     rightEncoderCount = rightEncoderCount + 1;
@@ -37,23 +33,27 @@
 //rotate the pan/tilt servos
 void RotateServos()
 {
-    if(xpadState == 1)
+    float topServoPos = 0.0;
+    float bottomServoPos = 0.0;
+    
+    while(1){
+    if(xpadState == 1) //pan right
     {
         bottomServoPos = bottomServoPos + 0.05;
         if(bottomServoPos > 1.0) bottomServoPos = 1.0;
     }
-    else if(xpadState == -1)
+    else if(xpadState == -1) //pan left
     {
         bottomServoPos = bottomServoPos - 0.05;
         if(bottomServoPos < 0.0) bottomServoPos = 0.0;
     }
     
-    if(ypadState == 1)
+    if(ypadState == 1) //tilt up
     {
         topServoPos = topServoPos + 0.05;
         if(topServoPos > 1.0) topServoPos = 1.0;
     }
-    else if(xpadState == -1)
+    else if(ypadState == -1) //tilt down
     {
         topServoPos = topServoPos - 0.05;
         if(topServoPos < 0.0) topServoPos = 0.0;
@@ -62,16 +62,38 @@
     topServo = topServoPos;
     bottomServo = bottomServoPos;
     Thread::wait(150);
+    }
 }
 
 
 //Sets and keeps motor speed matched using encoder output
 void SpeedBalance()
 {
+    bool interruptAttached = false; //flag indicating whether encoders are being counted
+    bool speedSet = false; //flag indicating if the inial motor speed has been set
+    
+    //used when speed adjusting using encoders
+    float leftMotorSpeed = 0.0;
+    float rightMotorSpeed = 0.0;
+    
+    int encoderCountDifference = 0;
+    int countDifferenceTolerance = 2; //How many counts difference between encoders is allowed
+    
+    while(1){
     if(leftJoystickState != 0 && rightJoystickState != 0 && leftJoystickState == rightJoystickState) //only need to match speeds if they're moving in same direction
     {
+        if(speedSet == false) //set motor speed initally
+        {
+            leftMotorSpeed = leftJoystickState;
+            rightMotorSpeed = rightJoystickState;
+            speedSet = true;
+        }
+        
+        //set motor speed using variable rather than joystick state, because joystick is read-only
         leftMotor.speed(leftMotorSpeed);
         rightMotor.speed(rightMotorSpeed);
+        
+        //start counting encoders if not already
         if(interruptAttached == false)
         {
             leftEncoder.fall(&LeftCount);
@@ -79,24 +101,45 @@
             interruptAttached = true;
         }
         
-        if(leftEncoderCount > rightEncoderCount)
+        encoderCountDifference = leftEncoderCount - rightEncoderCount;
+        if(encoderCountDifference > countDifferenceTolerance) //if left encoder counted more, left motor is faster
         {
-            leftMotor.speed(leftMotor.ReadSpeed() - 0.01);
-            leftEncoderCount = 0;
+            //add or subtract motor speed, depending on if its positive or negative
+            if(leftMotorSpeed < 0){
+                leftMotorSpeed = leftMotorSpeed + 0.01;
+                leftMotor.speed(leftMotorSpeed);
+            }
+            else if(leftMotorSpeed > 0){
+                leftMotorSpeed = leftMotorSpeed - 0.01;
+                leftMotor.speed(leftMotorSpeed);
+            }
+            leftEncoderCount = 0; //reset encoder counts
             rightEncoderCount = 0;
         }
-        else if(leftEncoderCount < rightEncoderCount)
+        else if(encoderCountDifference < (-1 * countDifferenceTolerance)) //if left encoder counted less, right motor is faster
         {
-            rightMotor.speed(rightMotor.ReadSpeed() - 0.01);
-            leftEncoderCount = 0;
+            //add or subtract motor speed, depending on if its positive or negative
+            if(leftMotorSpeed < 0){
+                rightMotorSpeed = rightMotorSpeed + 0.01;
+                rightMotor.speed(leftMotorSpeed);
+            }
+            else if(leftMotorSpeed > 0){
+                rightMotorSpeed = rightMotorSpeed - 0.01;
+                rightMotor.speed(leftMotorSpeed);
+            }
+            leftEncoderCount = 0; //reset encoder counts
             rightEncoderCount = 0;
         }
         else Thread::yield(); //if counts are same, nothing left to do
     }
-    else //stop counting encoders when motor speeds unequal
+    else //The case where joysticks arent in the same direction
     {
+        //set motors using joystick states, since motor speed wont need to be dynamically changed in this case
         leftMotor.speed(leftJoystickState);
         rightMotor.speed(rightJoystickState);
+        speedSet = false;
+        
+        //Stop counting encoders because they aren't being used
         if(interruptAttached == true)
         {
             leftEncoder.fall(NULL);
@@ -106,6 +149,7 @@
             rightEncoderCount = 0;
         }
     }
+    }
 }
 
 int main() 
@@ -121,46 +165,46 @@
     {
         if(pi.readable()) //check if new command from pi available
         {
-            if (pi.getc() == '!' ) {
+            if (pi.getc() == '!' ) { //look for dpad update
                 servoNum = pi.getc();
-                if (servoNum == '0') {
+                if (servoNum == '0') { //dpad released
                     xpadState = 0;
                     ypadState = 0;
                 }
-                else if (servoNum == '1') {
+                else if (servoNum == '1') { //dpad left
                     xpadState = -1;
                 }
-                else if (servoNum == '2') {
+                else if (servoNum == '2') { //dpad right
                     xpadState = 1;
                 }
-                else if (servoNum == '3') {
+                else if (servoNum == '3') { //dpad up
                     ypadState = 1;
                 }
-                else if (servoNum == '4') {
+                else if (servoNum == '4') { //dpad down
                     ypadState = -1;
                 }
             }
-            else if (pi.getc() == '(') {
+            else if (pi.getc() == '(') { //look for left joystick update
                 leftMotNum = pi.getc();
-                if (leftMotNum == '0') {
+                if (leftMotNum == '0') { //joystick center
                     leftJoystickState = 0;
                 }
-                else if (leftMotNum == '1') {
+                else if (leftMotNum == '1') { //joystick up
                     leftJoystickState = 1;
                 }
-                else if (leftMotNum == '2') {
+                else if (leftMotNum == '2') { //joystick down
                     leftJoystickState = -1;
                 }
             }
-            else if (pi.getc() == ')' ) {
+            else if (pi.getc() == ')' ) { //look for right joystick update
                 rightMotNum = pi.getc();
-                if (rightMotNum == '0') {
+                if (rightMotNum == '0') { //joystick center
                     rightJoystickState = 0;
                 }
-                else if (rightMotNum == '1') {
+                else if (rightMotNum == '1') { //joystick up
                     rightJoystickState = 1;
                 }
-                else if (rightMotNum == '2') {
+                else if (rightMotNum == '2') { //joystick down
                     rightJoystickState = -1;
                 }
             }