started writing code but not finished yet :(
This commit is contained in:
parent
1a690a3722
commit
b4eebea7de
422
src/main.cpp
422
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
|
||||
void handle_can_lv_message(const CAN_message_t &msg) {
|
||||
/* Callback for LV CAN messages */
|
||||
|
||||
// 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;
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
/* 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
|
||||
|
||||
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);*/
|
||||
}
|
||||
|
||||
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 */
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue