commit 91eb608abfd27ffd84e12489ad1f8481af70984f
Author: Nicholas Stanescu <nicholasstanes@tudelft.nl>
Date:   Fri May 22 17:16:45 2026 +0200

    Initial Commit

diff --git a/CAN-arduino.ino b/CAN-arduino.ino
new file mode 100644
index 0000000..fd23d25
--- /dev/null
+++ b/CAN-arduino.ino
@@ -0,0 +1,77 @@
+/**************************************************************
+ * TestVESC.ino
+ *
+ * Leon Abelmann, March 2026
+ *
+ * Version 1.0 - Set dutycyle, wait, stop, wait, repeat
+ * Information:
+ * https://github.com/waas-rent/vesc_can_sdk/blob/main/CAN_PROTOCOL.md
+ * https://github.com/vedderb/bldc/blob/master/documentation/comm_can.md
+ * Use VESC tool to set parameters on the VESC controller
+ * Load and save configurations using File menu
+ * Read and write configuration to VESC using M and A buttons on right
+ * VESC Dev Tools / Terminal is your friend, type help
+ **************************************************************/
+
+
+#include "CAN.h"
+#include "VESC_controller.h"
+
+/* Initialize CAN bus, ports match the Next! breakout board */
+mbed::CAN can1(PB_5, PB_13);
+
+/* VESC controllers . Read/Set with VESCTool software */
+VESC_controller VESC1 = VESC_controller(0x5A);  // Hex (0x5A = 90 decimal )
+VESC_controller VESC2 = VESC_controller(0x5B);  // Hex (0x5B = 91 decimal )
+
+int dutycycle1 = 30000; // Range is -100000 - 100000
+int dutycycle2 = 30000;
+
+/* Setup CAN bus and serial communcation */
+void setup() {
+  //switch relays over to VESC
+  pinMode(D10, OUTPUT);
+  digitalWrite(D10, LOW);
+
+// Setup CAN bus
+  can1.frequency(500000);
+
+  // Serial communication (for debugging)
+  Serial.begin(115200);
+  delay(1000);
+  Serial.write("VescControl is starting up!");
+
+
+}
+
+uint8_t serial_data;
+
+void loop() {
+  mbed::CANMessage canMsg;  
+  
+  while(Serial.available()){
+      Serial.readBytes(&serial_data,1);
+      Serial.print("found digit: ");
+      Serial.println(serial_data);
+      //dutycycle1 = serial_data;
+      VESC2.setDutyCycle(serial_data*784-100000);
+  }
+  
+  
+  // Print data of status message 1 received from VESC
+  // Switch on in VESC tool - General / CAN Message Rate
+  // Set frequency to 2 Hz for example
+  // Is there a message waiting on the bus?
+  while (can1.read(canMsg)) {
+    //Serial.println(canMsg.id);
+    // Check if the message is a status 1 message from VESC1 (0x0009)
+    if (canMsg.id == 0x00090000 | VESC1.getVescID()) {
+      VESC1.getStatus1(canMsg);
+    }
+    if (canMsg.id == 0x00090000 | VESC2.getVescID()) {
+      VESC2.getStatus1(canMsg);
+    }
+  }
+  delay(1000);
+}
+
diff --git a/VESC_controller.cpp b/VESC_controller.cpp
new file mode 100644
index 0000000..ed8e34b
--- /dev/null
+++ b/VESC_controller.cpp
@@ -0,0 +1,66 @@
+#include "VESC_controller.h"
+extern mbed::CAN can1; 
+
+int32_t VESC_controller::setDutyCycle(int32_t duty) {
+  mbed::CANMessage canMsg;
+  // VESC uses extended message IDs
+  // The message ID is the combination of the command and the VESC ID
+  // Set duty cycle = 0x0000, vescID = 0x5A for example, than
+  // combined ID = 0x0000005A
+  canMsg.id      = 0x000000000 | vescId;  
+  canMsg.format  = CANExtended;
+  canMsg.len     = 4;       // Message length
+  // Convert duty cycle to 4 bytes
+  // 32-bit big endian signed number
+  canMsg.data[3] = (duty >> 0) & 0xFF;
+  canMsg.data[2] = (duty >> 8) & 0xFF;
+  canMsg.data[1] = (duty >> 16) & 0xFF;
+  canMsg.data[0] = (duty >> 24) & 0xFF;
+
+  // Send message on CAN bus
+  can1.write(canMsg);
+
+  current_duty_cycle = duty;
+  return current_duty_cycle;
+}
+
+void VESC_controller::getStatus1(const mbed::CANMessage &canMsg) {
+  // For debug:
+  // // Serial.print("CAN Message Data: ");
+  // for (int i = 0; i < canMsg.len; i++) {
+  //   if (canMsg.data[i] < 0x10) // Serial.print("0"); // Print leading zero for single-digit hex
+  //   // Serial.print(canMsg.data[i], HEX);
+  //   // Serial.print(" ");
+  // }
+  // // Serial.println();
+
+  // Extract Current (bytes 4-5)
+  int16_t current_raw = (canMsg.data[4] << 8) | canMsg.data[5];
+  float current = current_raw / 10.0f; // Scale by 10
+
+  // Extract Duty Cycle (bytes 6-7)
+  int16_t duty_raw = (canMsg.data[6] << 8) | canMsg.data[7];
+  float duty = duty_raw / 1000.0f; // Scale by 1000
+
+  // Print results
+  // Serial.print("Current: ");
+  // Serial.print(current);
+  readout_motor_current = current;
+  // Serial.print(" A, Duty: ");
+  // Serial.print(duty*100);
+  readout_duty_cycle = duty*100;
+  // Serial.println("%");
+}
+
+bool VESC_controller::verifyDutyCycle(){
+    return readout_duty_cycle == current_duty_cycle;
+}
+
+void VESC_controller::getDataFromVesc(){
+  getStatus1(lastCanMessage);
+}
+
+uint8_t VESC_controller::getVescID(){
+
+  return vescId;
+}
diff --git a/VESC_controller.h b/VESC_controller.h
new file mode 100644
index 0000000..6b52a6e
--- /dev/null
+++ b/VESC_controller.h
@@ -0,0 +1,25 @@
+#include "CAN.h"
+
+class VESC_controller{
+  private:
+    uint8_t vescId =0;
+    int32_t current_duty_cycle=0;
+    int32_t readout_motor_current=0;
+    int32_t readout_duty_cycle=0;
+    mbed::CANMessage lastCanMessage;
+
+public:
+      int32_t setDutyCycle(int32_t duty_cycle);
+void getStatus1(const mbed::CANMessage &canMsg);
+      bool verifyDutyCycle();
+      void getDataFromVESC();
+      void getDataFromVesc();
+      uint8_t getVescID();
+    VESC_controller(uint8_t address){
+        vescId = address;
+        current_duty_cycle=0;
+        readout_motor_current=0;
+        readout_duty_cycle=0;
+      }
+
+};
\ No newline at end of file