Merge pull request #8 from LexxPluss/feature/us_charger

Feature/us charger (for FastCharge)

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;