67 lines
1.9 KiB
C++
67 lines
1.9 KiB
C++
#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;
|
|
}
|