From 1a690a3722838da07cc2a843bf7aec7140e1a979 Mon Sep 17 00:00:00 2001 From: bitscuit Date: Tue, 11 Apr 2023 14:13:36 +0200 Subject: [PATCH] added motor test code as starting point --- .gitignore | 3 + include/README | 39 +++++ platformio.ini | 14 ++ src/main.cpp | 393 +++++++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 449 insertions(+) create mode 100644 include/README create mode 100644 platformio.ini create mode 100644 src/main.cpp diff --git a/.gitignore b/.gitignore index a427a79..d6e7fe0 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,6 @@ +# Custom stuffs +.vscode + # ---> C # Prerequisites *.d diff --git a/include/README b/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/platformio.ini b/platformio.ini new file mode 100644 index 0000000..4adb7a6 --- /dev/null +++ b/platformio.ini @@ -0,0 +1,14 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:teensy40] +platform = teensy +board = teensy40 +framework = arduino diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..56ef661 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,393 @@ +#include +#include + +/*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 Can0; +elapsedMillis sinceSpeedUpdate; // counter voor snelheidssignaal +long speed; +bool analog = false; +bool manual = false; +bool stop = false; + + +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 + + 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 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 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); +} + + +// ID bij ons was 79 = 0x4F + +void sendEnable() { + CAN_message_t msg_DriveEnable; + msg_DriveEnable.flags.extended = 1; + msg_DriveEnable.id = 0x24 << 8 | 0x4F; // INVERTER ID INSTELLEN VOLGENS VOLGEND FORMAT: 0x24 << 8 | 0x"ID" + msg_DriveEnable.buf[0] = 1; + msg_DriveEnable.buf[1] = 255; + msg_DriveEnable.buf[2] = 0xFF; + msg_DriveEnable.buf[3] = 0xFF; + msg_DriveEnable.buf[4] = 0xFF; + msg_DriveEnable.buf[5] = 0xFF; + msg_DriveEnable.buf[6] = 0xFF; + msg_DriveEnable.buf[7] = 0xFF; + Can0.write(msg_DriveEnable); + Serial.println(); +} + + +void sendERPM() { + if ( millis() - time_last_update_can > 100 ) { + + sendEnable(); + Serial.print("Enable"); + + CAN_message_t msg_ERPM; + msg_ERPM.flags.extended = 1; + msg_ERPM.id = 0x1C << 8 | 0x4F; // INVERTER ID INSTELLEN VOLGENS VOLGEND FORMAT: 0x1C << 8 | 0x"ID" + msg_ERPM.buf[0] = (speed >> 24); + msg_ERPM.buf[1] = (speed >> 16) & 0xFF; + msg_ERPM.buf[2] = (speed >> 8) & 0xFF; + msg_ERPM.buf[3] = speed & 0xFF; + Can0.write(msg_ERPM); + Serial.println("ERPM"); + printMessage(msg_ERPM); + time_last_update_can = millis(); + } +} + + +void calculateSpeed(){ + uint64_t analogSpeed = analogRead(AnalogIn); + + speed = map(analogSpeed, 0, 1023, 0, 45000); + +} + + +void blinkLED(){ + digitalWrite(statusLED, HIGH); + delay(500); + digitalWrite(statusLED, LOW); + delay(500); +}