Diff: driving.h

Revision:

3:25c6bf0abc00

Parent:

1:c324a2849500

Child:

5:636c3fe18aa8

--- a/driving.h	Mon Oct 25 01:28:53 2021 +0000
+++ b/driving.h	Mon Oct 25 01:39:55 2021 +0000
@@ -16,3 +16,166 @@
 #define fullBatScalar 0.5873f
 #define speedSensorScalar 2.5f
 #define battDividerScalar 4.0;
+PwmOut motorLeft(PTB1); 
+PwmOut motorRight(PTB2);
+AnalogIn pot1(PTB0);
+AnalogIn speedSensorLeft(PTB3);
+AnalogIn speedSensorRight(PTC2);
+DigitalOut ledRed(LED1);
+AnalogIn battInput(PTC1);
+DigitalOut brakeLeft(PTA12);
+DigitalOut brakeRight(PTD4);
+
+DigitalIn enableBrakeLeft(PTA4);
+DigitalIn enableBrakeRight(PTA5);
+
+float pot1Voltage;
+float dutyCycleLeft;
+float tempDutyCycleLeft = 0;
+float tempDutyCycleRight = 0;
+float dutyCycleRight;
+float speedLeftVolt;
+float speedRightVolt;
+float batteryVoltage;
+float avgCellVoltage;
+
+float errorAreaLeft = 0;
+float errorAreaRight = 0;
+float errorAreaLeftPrev = 0;
+float errorAreaRightPrev = 0;
+float errorLeft = 0;
+float errorRight = 0;
+float setpointLeft = 0.0; //target speed, 0.0 to 1.0
+float setpointRight = 0.0;
+float controllerOutputLeft = 0;
+float controllerOutputRight = 0;
+float x;
+
+bool clampLeft = false;
+bool clampRight = false;
+
+bool brakeLeftBool = false;
+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;
+        }
+    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){ 
+brakeLeft = 1;
+    } else {
+        brakeLeft = 0;
+}
+
+if (controllerOutputRight < 0.0){ 
+brakeRight = 1;
+    } else {
+        brakeRight = 0;
+}
+*/
+        
+      
+}
\ No newline at end of file