integrating CAN into main

src/main.cpp

@@ -79,16 +79,35 @@ TorqueControllerMux torque_controller_mux;
 /* Declare state machine */
 MCUStateMachine<DrivetrainSystemType> fsm(&buzzer, &drivetrain, &dashboard, &pedals_system, &torque_controller_mux, &ams_interface);    // need more implemetation details. associated interfaces and systems tied by pointers
 
-/* Function declarations */
-/* CAN functions */
-void update_and_enqueue_all_CAN(const SysTick_s& current_system_tick);
-/* External value readings */
-void sample_all_external_readings(const SysTick_s& current_system_tick);
+CANInterfaces<CircularBufferType> CAN_interfaces = {&fl_inv, &fr_inv, &rl_inv, &rr_inv, &dashboard, &ams_interface};
+
+/* tick interfaces */
+void tick_all_interfaces(const SysTick_s& current_system_tick);
 /* Tick all systems */
 void tick_all_systems(const SysTick_s& current_system_tick);
 
-void setup() {
+void init_all_CAN_devices() {
+    // Inverter CAN line
+    INV_CAN.begin();
+    INV_CAN.setBaudRate(INV_CAN_BAUDRATE);
+    INV_CAN.setMaxMB(16);
+    INV_CAN.enableFIFO();
+    INV_CAN.enableFIFOInterrupt();
+    INV_CAN.onReceive(on_can2_receive);
+    INV_CAN.mailboxStatus();
 
+    // Telemetry CAN line
+    TELEM_CAN.begin();
+    TELEM_CAN.setBaudRate(TELEM_CAN_BAUDRATE);
+    TELEM_CAN.setMaxMB(16);
+    TELEM_CAN.enableFIFO();
+    TELEM_CAN.enableFIFOInterrupt();
+    TELEM_CAN.onReceive(on_can3_receive);
+    TELEM_CAN.mailboxStatus();
+}
+
+void setup() {
+    init_all_CAN_devices();
     /* Tick system clock */
     auto curr_tick = sys_clock.tick(micros());
 
@@ -102,83 +121,33 @@ void setup() {
     // ControllerMux set initial max torque to 10 NM, torque mode to 0 (could be done at construction or have an init function)
     // Drivetrain set all inverters disabled, write inv_en and inv_24V_en hight, set inverter_has_error to false if using
 
-    /* Present action for 5s */
-    delay(SETUP_PRESENT_ACTION_INTERVAL);
-
-    /* Set start up status */
-    // fsm either initialize state to STARTUP/TRACTIVE_SYSTEM_NOT_ACTIVE or enable external set state
+    // delay for 1 second
+    delay(1000);
     // ControllerMux set max torque to 21 NM, torque mode to whatever makes most sense
 
 }
 
 void loop() {
 
     auto cur_tick = sys_clock.tick(micros());
-    // Sensors sample and cache
-    sample_all_external_readings(cur_tick);
-
+    process_ring_buffer(CAN2_rxBuffer, CAN_interfaces, cur_tick.millis);
+    process_ring_buffer(CAN3_rxBuffer, CAN_interfaces, cur_tick.millis);
+    // tick interfaces
+    tick_all_interfaces(cur_tick);
     /* System process readings prior to ticking state machine */
-
     tick_all_systems(cur_tick);
     fsm.tick_state_machine(cur_tick.millis);
     /* Update and enqueue CAN messages */
-    update_and_enqueue_all_CAN(cur_tick);
 
     /* Inverter procedure before entering state machine */
     // Drivetrain check if inverters have error
     // Drivetrain reset inverters
 
-    /* Tick state machine */
-    // fsm.tick_state_machine(curr_tick);
-
     /* Tick safety system */
     safety_system.software_shutdown(cur_tick);
 }
 
-void init_all_CAN() {
-    // Inverter CAN line
-    INV_CAN.begin();
-    INV_CAN.setBaudRate(INV_CAN_BAUDRATE);
-    INV_CAN.setMaxMB(16);
-    INV_CAN.enableFIFO();
-    INV_CAN.enableFIFOInterrupt();
-    INV_CAN.onReceive(on_can2_receive);
-    INV_CAN.mailboxStatus();
-
-    // Telemetry CAN line
-    TELEM_CAN.begin();
-    TELEM_CAN.setBaudRate(TELEM_CAN_BAUDRATE);
-    TELEM_CAN.setMaxMB(16);
-    TELEM_CAN.enableFIFO();
-    TELEM_CAN.enableFIFOInterrupt();
-    TELEM_CAN.onReceive(on_can3_receive);
-    TELEM_CAN.mailboxStatus();
-
-    delay(500);
-}
-
-void update_and_enqueue_all_CAN(const SysTick_s& current_system_tick) {
-    // Drivetrain system
-        // probably here as well
-    // MCU interface
-    // main_ecu.tick(curr_tick,
-                //   fsm.get_state(),
-                //   drivetrain.drivetrain_error_occured(),
-                //   safety_system.get_software_is_ok(),
-                  // TCMux return
-                //   buzzer.buzzer_is_on(),
-                  // Pedal system return
-                //   ams_interface.pack_charge_is_critical(),
-                //   dash.lauchControlButtonPressed());
-    // Telemetry
-    telem_interface.tick(current_system_tick,
-                         ADC1.get(),
-                         ADC2.get(),    // Add MCP3204 functionality for corner board
-                         ADC3.get(),    // Add implementation to get()
-                         steering1.convert());
-}
-
-void sample_all_external_readings(const SysTick_s& current_system_tick) {
+void tick_all_interfaces(const SysTick_s& current_system_tick) {
     // Tick all adcs
     ADC1.tick(current_system_tick);
     ADC2.tick(current_system_tick);
@@ -187,6 +156,11 @@ void sample_all_external_readings(const SysTick_s& current_system_tick) {
     steering_system.tick(current_system_tick, ADC1.get().conversions[MCU15_STEERING_CHANNEL]);
     // Read shutdown circuits    
     main_ecu.read_mcu_status();
+    telem_interface.tick(current_system_tick,
+                         ADC1.get(),
+                         ADC2.get(),    // Add MCP3204 functionality for corner board
+                         ADC3.get(),    // Add implementation to get()
+                         steering1.convert());
 }
 
 void tick_all_systems(const SysTick_s& current_system_tick) {
@@ -204,7 +178,7 @@ void tick_all_systems(const SysTick_s& current_system_tick) {
     drivetrain.tick(current_system_tick);
     torque_controller_mux.tick(
         current_system_tick,
-        drivetrain.get_current_data(),       // TODO: get drivetrain dynamic data
+        drivetrain.get_current_data(),
         pedals_system.getPedalsSystemData(),
         steering_system.getSteeringSystemData(),
         ADC2.get().conversions[0],                  // FL load cell reading. TODO: fix index