Diff: driving.h

Revision:

5:636c3fe18aa8

Parent:

3:25c6bf0abc00

Child:

6:d2bd68ba99c9

--- a/driving.h	Mon Oct 25 01:53:10 2021 +0000
+++ b/driving.h	Mon Oct 25 02:14:11 2021 +0000
@@ -58,110 +58,114 @@
 bool brakeRightBool = false;
 
 
- 
-    
-void PI() { //motor PI interrupt
-
-//--- Calculate Error ---
-errorLeft = setpointLeft - (speedLeftVolt / 3.3f); //error and setpoint is defined as a percentage, from 0 to 1
-errorRight = setpointRight - (speedRightVolt / 3.3f);
-
-//--- Steady State Error Tolerace ---
-if (abs(errorLeft) < 0.01){
-    errorLeft = 0.0;
-    }
-if (abs(errorRight) < 0.01){
-    errorRight = 0.0;
-    }
-//--- Calculate integral error ---
-if (clampLeft == false){
-errorAreaLeft = TI*errorLeft + errorAreaLeftPrev;
-}
-
-if (clampRight == false){ 
-errorAreaRight = TI*errorRight + errorAreaRightPrev;
-}
-
-errorAreaLeftPrev = errorAreaLeft;
-errorAreaRightPrev = errorAreaRight;
-
-/*
-if (errorAreaLeft > 0.1){
-   errorAreaLeft = 0.0;
-   }
-   p
-if (errorAreaRight > 0.1){
-errorAreaRight = 0.0;
-}         
-*/       
-   
-//--- Calculate total error ---  
-controllerOutputLeft = KP_LEFT*errorLeft + KI_LEFT*errorAreaLeft;
-controllerOutputRight = KP_RIGHT*errorRight + KI_RIGHT*errorAreaRight;
-
-tempDutyCycleLeft = fullBatScalar * controllerOutputLeft;
-tempDutyCycleRight = fullBatScalar * controllerOutputRight;
-
-
-//--- Motor over-voltage safety ---
-if (tempDutyCycleLeft > fullBatScalar){ //safety mechanism in case feedback breaks for any reason - 
-        dutyCycleLeft = fullBatScalar; //will stop output from exceeding max duty cycle and damaging motor
-    } else {                                            
-        dutyCycleLeft = tempDutyCycleLeft;
-}
-
-if (tempDutyCycleRight > fullBatScalar){
-        dutyCycleRight = fullBatScalar;
-    } else {
-        dutyCycleRight = tempDutyCycleRight;
-    }
-    
-
-//--- integral anti-windup ---
-if (controllerOutputLeft > 1.0){
-    if (errorAreaLeft > 0.0){
-        clampLeft = true;
+volatile bool motor_enabled = true;
+void PI(void) { //motor PI interrupt
+if(motor_enabled == true) {
+            //--- Calculate Error ---
+            errorLeft = setpointLeft - (speedLeftVolt / 3.3f); //error and setpoint is defined as a percentage, from 0 to 1
+            errorRight = setpointRight - (speedRightVolt / 3.3f);
+            
+            //--- Steady State Error Tolerace ---
+            if (abs(errorLeft) < 0.01){
+                errorLeft = 0.0;
+                }
+            if (abs(errorRight) < 0.01){
+                errorRight = 0.0;
+                }
+            //--- Calculate integral error ---
+            if (clampLeft == false){
+            errorAreaLeft = TI*errorLeft + errorAreaLeftPrev;
+            }
+            
+            if (clampRight == false){ 
+            errorAreaRight = TI*errorRight + errorAreaRightPrev;
+            }
+            
+            errorAreaLeftPrev = errorAreaLeft;
+            errorAreaRightPrev = errorAreaRight;
+            
+            /*
+            if (errorAreaLeft > 0.1){
+               errorAreaLeft = 0.0;
+               }
+               p
+            if (errorAreaRight > 0.1){
+            errorAreaRight = 0.0;
+            }         
+            */       
+               
+            //--- Calculate total error ---  
+            controllerOutputLeft = KP_LEFT*errorLeft + KI_LEFT*errorAreaLeft;
+            controllerOutputRight = KP_RIGHT*errorRight + KI_RIGHT*errorAreaRight;
+            
+            tempDutyCycleLeft = fullBatScalar * controllerOutputLeft;
+            tempDutyCycleRight = fullBatScalar * controllerOutputRight;
+            
+            
+            //--- Motor over-voltage safety ---
+            if (tempDutyCycleLeft > fullBatScalar){ //safety mechanism in case feedback breaks for any reason - 
+                    dutyCycleLeft = fullBatScalar; //will stop output from exceeding max duty cycle and damaging motor
+                } else {                                            
+                    dutyCycleLeft = tempDutyCycleLeft;
+            }
+            
+            if (tempDutyCycleRight > fullBatScalar){
+                    dutyCycleRight = fullBatScalar;
+                } else {
+                    dutyCycleRight = tempDutyCycleRight;
+                }
+                
+            
+            //--- integral anti-windup ---
+            if (controllerOutputLeft > 1.0){
+                if (errorAreaLeft > 0.0){
+                    clampLeft = true;
+                    }
+                if (errorAreaLeft < 0.0){
+                    clampLeft = false;
+                    }
+                } else {
+                    clampLeft = false;
+            }
+            
+            if (controllerOutputRight > 1.0){
+                if (errorAreaRight > 0.0){
+                    clampRight = true;
+                    }
+                if (errorAreaRight < 0.0){
+                    clampRight = false;
+                    }
+                } else {
+                    clampRight = false;
+            }
+            
+            //--- fucked up manual braking stuff ---
+            if (setpointLeft == 0.0 || brakeLeftBool == true)
+            {
+                errorAreaLeft = 0.0;
+                brakeLeft = 1;
+            } else {
+                brakeLeft = 0;
+            }
+            
+            if (setpointRight == 0.0 || brakeRightBool == true)
+            {
+                errorAreaRight = 0.0;
+                brakeRight = 1;
+            } else {
+                brakeRight = 0;
+            }
+            
+            //--- set motors to calculated output ---    
+            motorLeft.write(dutyCycleLeft);
+            motorRight.write(dutyCycleRight);
+            }
+    else if(motor_enabled == false) {
+            // Does duty cycle zero turn off the motors? 
+            motorLeft.write(0);
+            motorRight.write(0);
+        
         }
-    if (errorAreaLeft < 0.0){
-        clampLeft = false;
-        }
-    } else {
-        clampLeft = false;
-}
-
-if (controllerOutputRight > 1.0){
-    if (errorAreaRight > 0.0){
-        clampRight = true;
-        }
-    if (errorAreaRight < 0.0){
-        clampRight = false;
-        }
-    } else {
-        clampRight = false;
-}
-
-//--- fucked up manual braking stuff ---
-if (setpointLeft == 0.0 || brakeLeftBool == true)
-{
-    errorAreaLeft = 0.0;
-    brakeLeft = 1;
-} else {
-    brakeLeft = 0;
-}
-
-if (setpointRight == 0.0 || brakeRightBool == true)
-{
-    errorAreaRight = 0.0;
-    brakeRight = 1;
-} else {
-    brakeRight = 0;
-}
-
-//--- set motors to calculated output ---    
-motorLeft.write(dutyCycleLeft);
-motorRight.write(dutyCycleRight);
-
-
 //--- motor braking ---
 /*
 if (controllerOutputLeft < 0.0){