Added control pin toggling for overcurrent

MissouriMRDT · Apr 26, 2022 · 6ad5473 · 6ad5473
1 parent 54f6036
commit 6ad5473
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 9 deletions.
diff --git a/powerboardcode/powerboard/powerboard.h b/powerboardcode/powerboard/powerboard.h
@@ -23,9 +23,9 @@ IntervalTimer Telemetry;
 #define OVERCURRENT_PACK        19500 //mA
 #define OVERCURRENT_12V         4800 //mA
 #define CURRENT_ADC_MIN         0
-#define CURRENT_ADC_MAX         0
+#define CURRENT_ADC_MAX         4096
 #define CURRENT_mA_MIN          0
-#define CURRENT_mA_MAX          0
+#define CURRENT_mA_MAX          20000
 
 //12V CTL
 #define SPARE_CTL               0
@@ -51,8 +51,8 @@ IntervalTimer Telemetry;
 
 // Current sensing variables
 
-float MOTORBUSPINS[7] = {P_MOTOR1_SENSE, P_MOTOR2_SENSE, P_MOTOR3_SENSE, P_MOTOR4_SENSE, P_MOTOR5_SENSE, P_MOTOR6_SENSE, P_MOTOR7_SENSE};
-float TWELVELOGICBUSPINS[8] = {GIMBAL_ACT_SENSE, DRIVE_SENSE, MULTIMEDIA_SENSE, NAV_SENSE, CAM1_SENSE, CAM2_SENSE, BBB_SENSE, SPARE_12V_SENSE};
+float MOTORBUSSENSEPINS[NUM_MOTORS] = {P_MOTOR1_SENSE, P_MOTOR2_SENSE, P_MOTOR3_SENSE, P_MOTOR4_SENSE, P_MOTOR5_SENSE, P_MOTOR6_SENSE, P_MOTOR7_SENSE};
+float TWELVELOGICBUSPINS[NUM_12V_PORTS] = {GIMBAL_ACT_SENSE, DRIVE_SENSE, MULTIMEDIA_SENSE, NAV_SENSE, CAM1_SENSE, CAM2_SENSE, BBB_SENSE, SPARE_12V_SENSE};
 
 float P_MOTOR1_CURRENT = 0;
 float P_MOTOR2_CURRENT = 0;
@@ -73,8 +73,8 @@ float CAM2_CURRENT = 0;
 float BBB_CURRENT = 0;
 float SPARE_12V_CURRENT = 0;
 
-float MOTORBUSCURRENTS[7] = {P_MOTOR1_CURRENT, P_MOTOR2_CURRENT, P_MOTOR3_CURRENT, P_MOTOR4_CURRENT, P_MOTOR5_CURRENT, P_MOTOR6_CURRENT, P_MOTOR7_CURRENT};
-float TWELVELOGICBUSCURRENTS[8] = {GIMBAL_ACT_CURRENT, DRIVE_CURRENT, MULTIMEDIA_CURRENT, NAV_CURRENT, CAM1_CURRENT, CAM2_CURRENT, BBB_CURRENT, SPARE_12V_CURRENT};
+float MOTORBUSCURRENTS[NUM_MOTORS] = {P_MOTOR1_CURRENT, P_MOTOR2_CURRENT, P_MOTOR3_CURRENT, P_MOTOR4_CURRENT, P_MOTOR5_CURRENT, P_MOTOR6_CURRENT, P_MOTOR7_CURRENT};
+float TWELVELOGICBUSCURRENTS[NUM_12V_PORTS] = {GIMBAL_ACT_CURRENT, DRIVE_CURRENT, MULTIMEDIA_CURRENT, NAV_CURRENT, CAM1_CURRENT, CAM2_CURRENT, BBB_CURRENT, SPARE_12V_CURRENT};
 
 uint8_t motorOverCurrent = 0;
 uint8_t twelveActOverCurrent = 0;

diff --git a/powerboardcode/powerboard/powerboard.ino b/powerboardcode/powerboard/powerboard.ino
@@ -2,7 +2,7 @@
 void setup() 
 {
     Serial.begin(115200);
-    Telemetry.begin(telemetry, 150000);
+    Telemetry.begin(telemetry, 1500000);
     setPins();
     setPinStates();
     Serial.begin(115200);
@@ -140,11 +140,14 @@ void measureCurrent()
 {
     for (uint8_t i = 0; i < 7; i++)
     {
-        MOTORBUSCURRENTS[i] = senseCurrent(MOTORBUSPINS[i]);
+        MOTORBUSCURRENTS[i] = senseCurrent(MOTORBUSSENSEPINS[i]);
 
         if (MOTORBUSCURRENTS[i] > OVERCURRENT_PACK)
         {
             motorOverCurrent |= (1 << i);
+            digitalWrite(motorBusses[i], LOW);
+            delay(1000);
+            digitalWrite(motorBusses[i], HIGH);
         }
         else
         {
@@ -156,6 +159,9 @@ void measureCurrent()
     if (AUX_CURRENT > OVERCURRENT_12V)
     {
         twelveActOverCurrent = 1;
+        digitalWrite(AUX_CTL, LOW);
+        delay(1000);
+        digitalWrite(AUX_CTL, HIGH);
     }
     else
     {
@@ -169,6 +175,9 @@ void measureCurrent()
         if (TWELVELOGICBUSCURRENTS[i] > OVERCURRENT_12V)
         {
             twelveLogicOverCurrent |= (1 << i);
+            digitalWrite(twelveVoltBusses[i], LOW);
+            delay(1000);
+            digitalWrite(twelveVoltBusses[i], HIGH);
         }
         else
         {
@@ -203,5 +212,5 @@ void telemetry()
     delay(100);
     RoveComm.write(RC_POWERBOARD_TWELVEVLOGICBUSCURRENT_DATA_ID, RC_POWERBOARD_TWELVEVLOGICBUSCURRENT_DATA_COUNT, TWELVELOGICBUSCURRENTS);
     delay(100);
-    Telemetry.begin(telemetry, 150000);
+    Telemetry.begin(telemetry, 1500000);
 }