Merge pull request #8 from LexxPluss/feature/us_charger

Feature/us charger (for FastCharge)
LexxPluss · Apr 25, 2024 · e91418b · e91418b
2 parents 5266819 + ff7d2a4
commit e91418b
Showing 1 changed file with 23 additions and 4 deletions.
diff --git a/main.cpp b/main.cpp
@@ -399,6 +399,15 @@ class auto_charger { // Variables Half-Implemented (Not Thermistors ADC)
         set_enable(false);
         send_heartbeat();
     }
+    bool is_charger_ready() const {
+        if (connector_v > (CHARGING_VOLTAGE * 0.9)) {
+            LOG("charger ready voltage:%f.\n", connector_v);
+            return true;
+        } else {
+            LOG("connector_v:%f THRESH:%f\n", connector_v, (CHARGING_VOLTAGE * 0.9));
+            return false;
+        }
+    }
     void get_connector_temperature(int &positive, int &negative) const {
         positive = clamp(static_cast<int>(connector_temp[0]), 0, 255);
         negative = clamp(static_cast<int>(connector_temp[1]), 0, 255);
@@ -475,7 +484,15 @@ class auto_charger { // Variables Half-Implemented (Not Thermistors ADC)
             send_heartbeat();
     }
     void send_heartbeat() {                                                    /* Creates the message to send to the robot using the "compose" function below */
-        uint8_t buf[8], param[3]{++heartbeat_counter, static_cast<uint8_t>(sw.read()), rsoc}; // Message composed of 8 bytes, 3 bytes parameters -- Declaration
+        uint8_t sw_state{0};
+
+        if (is_connected()) {
+            sw_state = 1;
+        } else {
+            sw_state = 0;
+        }
+
+        uint8_t buf[8], param[3]{++heartbeat_counter, sw_state, rsoc}; // Message composed of 8 bytes, 3 bytes parameters -- Declaration
         serial_message::compose(buf, serial_message::HEARTBEAT, param);
 #ifndef SERIAL_DEBUG
         serial.write(buf, sizeof buf);
@@ -882,8 +899,10 @@ class state_controller { // Variables Implemented
                 LOG("plugged to manual charger\n");
                 set_new_state(POWER_STATE::MANUAL_CHARGE);
             } else if (!charge_guard_asserted && ac.is_docked() && bmu.is_chargable()) {
-                LOG("docked to auto charger\n");
-                set_new_state(POWER_STATE::AUTO_CHARGE);
+                if (ac.is_charger_ready() == true) {
+                    LOG("docked to auto charger\n");
+                    set_new_state(POWER_STATE::AUTO_CHARGE);
+                }
             }
             break;
         case POWER_STATE::AUTO_CHARGE:
@@ -1105,7 +1124,7 @@ class state_controller { // Variables Implemented
         watchdog.kick();
     }
     void poll_10s() {
-        uint8_t buf[8]{'2', '1', '0'}; // version
+        uint8_t buf[8]{'2', '2', '0'}; // version
         can.send(CANMessage{0x203, buf});
     }
     can_driver can;