/**************************************************************
 * 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);
}