From b4eebea7deb946cc14628d0d7c2c2b3e39ce38d0 Mon Sep 17 00:00:00 2001
From: bitscuit <thomas@bitscuit.be>
Date: Tue, 11 Apr 2023 19:19:55 +0200
Subject: [PATCH] started writing code but not finished yet :(

---
 src/main.cpp | 426 ++++++++++++---------------------------------------
 1 file changed, 96 insertions(+), 330 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index 56ef661..36a4584 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,339 +1,71 @@
 #include <Arduino.h>
 #include <FlexCAN_T4.h>
 
-/*CODE BY TIM VAN HAUWERMEIREN 2022*/
-/*SLIGHT CHANGES BY NIELS DANZON 2023*/
-
-#define FILTER_ID -1
-/*
-0: ERPM, Duty Cycle, Input Voltage
-1: AC Current, DC Current
-2: Controller temperature, Motor temperature, Fault code
-3: Id, Iq
-4: Throttle Signal, Brake Signal, DI1, DI2, DI3, DI4, DO1, DO2, DO3, DO4, Drive Enable, Capacitor Temp Limit, DC Current Limit,
-Drive Enable Limit, IGBT Acceleration Temp Limit, IGBT Temp Limit, Input Voltage Limit, Motor Acceleration Temp Limit, Motor Temp Limit, RPM Min Limit, RPM Max Limit, Power Limit
-*/
-
-//Pin Declaration
-#define AnalogIn 17
-#define statusLED 13 
-#define AnalogAct 12
-#define ManualAct 11
-#define Stop 10
-
-//Set Speed value used in Set Mode
-#define setSpeed 500
-
 //Function Declaration
-void printMessage(const CAN_message_t *); // function for writing to the terminal
-void setupCan();
 void sendEnable(); // function to turn the inverter on DON'T FORGET TO EDIT THE ID OF THE DRIVE
 void sendERPM(); // function to change the speed DON'T FORGET TO EDIT THE ID OF THE DRIVE
-void calculateSpeed();
-void blinkLED();
 
-//Variable Declaration
-uint64_t time_last_update;
-uint64_t time_last_update_can;
-FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> Can0;
-elapsedMillis sinceSpeedUpdate; // counter voor snelheidssignaal
-long speed;
-bool analog = false;
-bool manual = false;
-bool stop = false;
+/* CAN variables */
+FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> can_lv;  /* LV CAN bus is connected to controller 1 */
+FlexCAN_T4<CAN2, RX_SIZE_256, TX_SIZE_16> can_mc;  /* Motorcontroller/Invertor CAN bus is connected to controller 2 */
+#define CAN_LV_BAUD 1000000                        /* Data rate of the LV CAN = 1Mbps */
+#define CAN_MC_BAUD 500000                         /* Data rate of the MC CAN = 500kbps */
+uint64_t time_last_can_mc_send = 0;                /* Timestamp of last message that was sent to motorcontrollers (because they have timeout safety shit) */
+uint64_t time_last_apps = 0;                       /* Timestamp of last message for APPS values */
+uint64_t time_last_rtd_state = 0;                  /* Timestamp of last message for Boot Sequence State stuff */
+
+/* CAN heartbeat stuff */
+uint64_t time_last_heartbeat = 0;  /* Timestamp of last heartbeat message that was sent */
+#define HEARTBEAT_PERIOD 100       /* Send heartbeat message every 100ms */
+#define CAN_ID_HEARTBEAT 0x080     /* CAN ID of heartbeat messages (see spreadsheet) */
+#define CAN_MODULE_ID 0x05         /* Module ID of this system (see spreadsheet) */
+
+/* CAN config continued */
+#define CAN_ID_APPS      0x110  /* CAN ID of APPS values (not using Module ID of APPS) */
+#define CAN_ID_BOOT_SEQ  0x090  /* CAN ID of Boot Sequence Status (for RTD) values (using Module ID of ECU for safety) */
+
+/* Torque Controller variables */
+uint8_t apps_value = 0;  /* Value of the APPS sensor (0-255) */
+char rtd_state = 0;      /* Whether or not RTD is actived */
 
 
-void setup() {
-  // put your setup code here, to run once:
-  setupCan();
-  pinMode(statusLED, OUTPUT);
-  pinMode(AnalogIn, INPUT);
-  pinMode(AnalogAct, INPUT_PULLUP);
-  pinMode(ManualAct, INPUT_PULLUP);
-  pinMode(Stop, INPUT_PULLUP);
-  Serial.print("Over 5 sec: test starten");
-  delay(5000); // 5 seconden wachten
-  
-  // onderstaande code geschreven door Niels Danzon (nog niet getest dus als het niet werkt aanpassen of die van Tim gebruiken)
-  speed = 1000; // Set ERPM=RPM*number of poles --> RPM = ERPM/10 (for emrax 208)
-  sendERPM();
-  sinceSpeedUpdate = 0;
-  while (speed <= 50000)
-  {
-    sendERPM();
-    if (sinceSpeedUpdate >= 2000)
-    {
-      sinceSpeedUpdate = 0;
-      speed += 1000;
-    }
-  }
-  
-  /* OLD CODE BY TIM VAN HAUWERMEIREN: 
-  problem with code is that it constantly accelerates and deaccelerates the motor because of the delay between the commands
+void handle_can_lv_message(const CAN_message_t &msg) {
+	/* Callback for LV CAN messages */
 
-  delay(10000); 
-  speed = 1000;
-  sendERPM();
-  delay(2000);
-  speed = 2000;
-  sendERPM();
-  delay(2000);
-  speed = 3000;
-  sendERPM();
-  speed = 4000;
-  sendERPM();
-  delay(2000);
-  speed = 5000;
-  sendERPM();
-  delay(2000);
-  speed = 6000;
-  sendERPM();
-  delay(2000);
-  speed = 7000;
-  sendERPM();
-  delay(2000);
-  speed = 8000;
-  sendERPM();
-  delay(2000);
-  speed = 9000;
-  sendERPM();
-  delay(2000);
-  speed = 10000;
-  sendERPM();
-  delay(2000);
-  speed = 11000;
-  sendERPM();
-  delay(2000);
-  speed = 12000;
-  sendERPM();
-  delay(2000);
-  speed = 13000;
-  sendERPM();
-  delay(2000);
-  speed = 14000;
-  sendERPM();
-  delay(2000);
-  speed = 15000;
-  sendERPM();
-  speed = 16000;
-  sendERPM();
-  delay(2000);
-  speed = 17000;
-  sendERPM();
-  delay(2000);
-  speed = 18000;
-  sendERPM();
-  delay(2000);
-  speed = 19000;
-  sendERPM();
-  delay(2000);
-  speed = 20000;
-  sendERPM();
-  delay(2000);
-  speed = 21000;
-  sendERPM();
-  delay(2000);
-  speed = 22000;
-  sendERPM();
-  delay(2000);
-  speed = 23000;
-  sendERPM();
-  delay(2000);
-  speed = 24000;
-  sendERPM();
-  delay(2000);
-  speed = 25000;
-  sendERPM();
-  speed = 26000;
-  sendERPM();
-  delay(2000);
-  speed = 27000;
-  sendERPM();
-  delay(2000);
-  speed = 28000;
-  sendERPM();
-  delay(2000);
-  speed = 29000;
-  sendERPM();
-  delay(2000);
-  speed = 30000;
-  sendERPM();
-  delay(2000);
-  speed = 31000;
-  sendERPM();
-  delay(2000);
-  speed = 32000;
-  sendERPM();
-  delay(2000);
-  speed = 33000;
-  sendERPM();
-  speed = 34000;
-  sendERPM();
-  delay(2000);
-  speed = 35000;
-  sendERPM();
-  delay(2000);
-  speed = 36000;
-  sendERPM();
-  delay(2000);
-  speed = 37000;
-  sendERPM();
-  delay(2000);
-  speed = 38000;
-  sendERPM();
-  delay(2000);
-  speed = 39000;
-  sendERPM();
-  delay(2000);
-  speed = 40000;
-  sendERPM();
-  delay(2000);
-  speed = 41000;
-  sendERPM();
-  delay(2000);
-  speed = 41500;
-  sendERPM();
-  delay(2000);
-  speed = 42000;
-  sendERPM();
-  delay(2000);
-  speed = 42500;
-  sendERPM();
-  delay(2000);
-  speed = 43000;
-  sendERPM();
-  delay(2000);
-  speed = 43500;
-  sendERPM();
-  delay(2000);
-  speed = 44000;
-  sendERPM();
-  delay(2000);
-  speed = 44500;
-  sendERPM();
-  delay(2000);
-  speed = 45000;
-  sendERPM();
-  delay(2000);
-  speed = 46000;
-  sendERPM();
-  delay(2000);
-  speed = 47000;
-  sendERPM();
-  delay(2000);
-  speed = 48000;
-  sendERPM();
-  delay(2000);
-  speed = 49000;
-  sendERPM();
-  delay(2000);
-  speed = 50000;
-  sendERPM();
-  delay(2000);*/
+	if ((msg.id & 0xff0) == CAN_ID_APPS) {
+		/* APPS value */
+		apps_value = msg.buf[0];
+		time_last_apps = millis();
+	}
+	else if ((msg.id & 0xfff) == CAN_ID_BOOT_SEQ) {
+		/* Boot Sequence Status */
+		char bs = msg.buf[0];
+		rtd_state = (bs == 5) ? 0b1 : 0b0;
+		time_last_rtd_state = millis();
+	}
 }
 
-void loop() { // no idea if this is still needed with the new code (I don't know C++)
-  // Wait Loop
-  while(!(analog || manual)){
-    stop = false;
-    //calculateSpeed();
-    
-    speed = 50000;
-    sendERPM();
-    //delay(500);
-    
-    
-
-    //blinkLED();
-    if(digitalRead(AnalogAct)){
-      analog = false;
-      
-    }
-    else{
-      analog = true;
-      Serial.print("Analog Mode\n");
-    }
-    if(digitalRead(ManualAct)){
-      manual = false;
-    }
-    else{
-      manual = true;
-      Serial.print("Manual Mode\n");
-    }
-    Serial.print("Waiting\n");
-    delay(1000);
-  }
-
-  //Analog Mode
-  while(analog){
-    calculateSpeed();
-    sendERPM();
-    if(digitalRead(Stop)){
-      analog = true;
-    }
-    else{
-      analog = false;
-    }
-    
-  }
-
-  //Set Mode
-  while(manual){
-    speed = 500;
-    sendERPM();
-    if(digitalRead(Stop)){
-      manual = true;
-    }
-    else{
-      manual = false;
-    }
-  }
+void handle_can_mc_message(const CAN_message_t &msg) {
+	/* Callback for MC CAN messages */
 }
 
 
-void printMessage(const CAN_message_t &msg) {
-  /*This function is called when a CAN message is received via the CAN bus. The message will be printed to the Serial for debugging purposes.
-  :param msg: Received Can message
-  :type msg: CAN_message_t
-  :return: void
-  :rtype: void
-  */
-  Serial.println("test");
-  if(msg.id == FILTER_ID || FILTER_ID == -1){
-    Serial.print("MB "); Serial.print(msg.mb);
-    Serial.print("  LEN: "); Serial.print(msg.len);
-    Serial.print(" TS: "); Serial.print(msg.timestamp);
-    Serial.print(" ID: "); Serial.print(msg.id, HEX);
-    Serial.print(" Buffer: ");
-    for ( uint8_t i = 0; i < msg.len; i++ ) {
-      Serial.print(msg.buf[i]); Serial.print(" ");
-    }
-    Serial.println();
-  }
-}
+void can_setup() {
+	/* Init LV CAN */
+	can_lv.begin();
+	can_lv.setBaudRate(CAN_LV_BAUD);
+	can_lv.setMaxMB(16);
+	can_lv.enableFIFO();
+	can_lv.enableFIFOInterrupt();
+	can_lv.onReceive(handle_can_lv_message);
 
-
-void setupCan() {
-  /*Initialize all CAN elements
-    :return: void
-    :rtype: void
-  */
-  Serial.begin(115200);
-  delay(400);
-  //Set up the clock and controller
-  Can0.begin();
-  //Set the symbol datarate to 1.000.000, TODO check max value
-  Can0.setBaudRate(500000);
-  //Set the maximal amount of MailBox'es (MB), an MB is a single buffer for a specific message type
-  //For transmitting frames on the CAN bus you can put them into a mailbox and they'll be scheduled to be sent when possible.
-  //Each mailbox is set either as a transmission or reception box.
-  Can0.setMaxMB(16);
-  // Enable FIFO, allows more than one instance of the same message to be stored before the CPU is interrupted
-  // therefore preventing any information loss for high frequency messages
-  Can0.enableFIFO();
-  // This allows frames to be interrupt driven rather than polling them in the loop.
-  Can0.enableFIFOInterrupt();
-  Can0.onReceive(printMessage);
+	/* Init MC CAN */
+	can_mc.begin();
+	can_mc.setBaudRate(CAN_MC_BAUD);
+	can_mc.setMaxMB(16);
+	can_mc.enableFIFO();
+	can_mc.enableFIFOInterrupt();
+	can_mc.onReceive(handle_can_mc_message);
 }
 
 
@@ -376,18 +108,52 @@ void sendERPM() {
   }
 }
 
+/* Subroutine to check if new heartbeat message needs to be sent and transmits it if needed */
+void check_heartbeat() {
+	/* Return if not needed yet */
+	if (millis() < time_last_heartbeat + HEARTBEAT_PERIOD) return;
 
-void calculateSpeed(){
-  uint64_t analogSpeed = analogRead(AnalogIn);
-  
-  speed = map(analogSpeed, 0, 1023, 0, 45000);
-  
+	/* Send heartbeat */
+	CAN_message_t m;
+	m.id = CAN_ID_HEARTBEAT | CAN_MODULE_ID;
+	m.buf[0] = HEARTBEAT_STATUS_OK;
+	can_lv.write(m);
+
+	/* Update stats */
+	time_last_heartbeat = millis();
 }
 
+/* Subroutine to update invertors */
+void update_drives() {
+	/* TODO: Return if not needed yet */
+	//if (millis() < time_last_heartbeat + HEARTBEAT_PERIOD) return;
 
-void blinkLED(){
-  digitalWrite(statusLED, HIGH);
-  delay(500);
-  digitalWrite(statusLED, LOW);
-  delay(500);
+	/* Update speed */
+	uint8_t speed = apps_value;
+
+	/* Check if data is recent enough */
+	//if (millis() > time_last_apps 
+
+	/* Send heartbeat */
+	CAN_message_t m;
+	m.id = CAN_ID_HEARTBEAT | CAN_MODULE_ID;
+	m.buf[0] = HEARTBEAT_STATUS_OK;
+	can_lv.write(m);
+
+	/* Update stats */
+	time_last_heartbeat = millis();
 }
+
+void setup() {
+	/* Init CAN */
+	can_setup();
+}
+
+void loop() {
+	/* Main event loop */
+	can_lv.events();    /* Handle CAN LV events */
+	can_mc.events();    /* Handle CAN MC events */
+	check_heartbeat();  /* Do heartbeat stuffz */
+	update_drives();    /* Do motorcontroller shit */
+}
+