#include "ClearCore.h"
#define motor ConnectorM0
#define baudRate 9600
#define SerialPort ConnectorUsb
#define HANDLE_MOTOR_FAULTS (0)
bool MoveToPosition(uint8_t positionNum);
void HandleMotorFaults();
int main() {
Connector::CPM_MODE_A_DIRECT_B_DIRECT);
motor.HlfbMode(MotorDriver::HLFB_MODE_HAS_BIPOLAR_PWM);
motor.HlfbCarrier(MotorDriver::HLFB_CARRIER_482_HZ);
motor.MotorInAState(false);
motor.MotorInBState(false);
SerialPort.Mode(Connector::USB_CDC);
SerialPort.Speed(baudRate);
uint32_t timeout = 5000;
SerialPort.PortOpen();
while (!SerialPort &&
Milliseconds() - startTime < timeout) {
continue;
}
motor.EnableRequest(true);
SerialPort.SendLine("Motor Enabled");
SerialPort.SendLine("Waiting for HLFB...");
while (motor.HlfbState() != MotorDriver::HLFB_ASSERTED &&
!motor.StatusReg().bit.MotorInFault) {
continue;
}
if (motor.StatusReg().bit.MotorInFault) {
SerialPort.SendLine("Motor fault detected.");
if(HANDLE_MOTOR_FAULTS){
HandleMotorFaults();
} else {
SerialPort.SendLine("Enable automatic fault handling by setting HANDLE_MOTOR_FAULTS to 1.");
}
SerialPort.SendLine("Enabling may not have completed as expected. Proceed with caution.");
SerialPort.SendLine();
} else {
SerialPort.SendLine("Motor Ready");
}
while (true) {
MoveToPosition(1);
MoveToPosition(2);
MoveToPosition(3);
MoveToPosition(4);
MoveToPosition(5);
MoveToPosition(6);
MoveToPosition(7);
MoveToPosition(8);
MoveToPosition(9);
MoveToPosition(10);
MoveToPosition(11);
MoveToPosition(12);
MoveToPosition(13);
MoveToPosition(14);
MoveToPosition(15);
MoveToPosition(16);
}
}
bool MoveToPosition(uint8_t positionNum) {
if (motor.StatusReg().bit.MotorInFault) {
if(HANDLE_MOTOR_FAULTS){
SerialPort.SendLine("Motor fault detected. Move canceled.");
HandleMotorFaults();
} else {
SerialPort.SendLine("Motor fault detected. Move canceled. Enable automatic fault handling by setting HANDLE_MOTOR_FAULTS to 1.");
}
return false;
}
SerialPort.Send("Moving to position: ");
SerialPort.SendLine(positionNum);
if (positionNum < 17 && positionNum > 0) {
for (uint8_t i = 0; i < positionNum; i++) {
motor.MotorInBState(true);
motor.MotorInBState(false);
}
motor.MotorInAState(true);
motor.MotorInAState(false);
}
else {
return false;
}
SerialPort.SendLine("Moving.. Waiting for HLFB");
while (motor.HlfbState() != MotorDriver::HLFB_ASSERTED &&
!motor.StatusReg().bit.MotorInFault) {
continue;
}
if (motor.StatusReg().bit.MotorInFault) {
SerialPort.SendLine("Motor fault detected.");
if(HANDLE_MOTOR_FAULTS){
HandleMotorFaults();
} else {
SerialPort.SendLine("Enable automatic fault handling by setting HANDLE_MOTOR_FAULTS to 1.");
}
SerialPort.SendLine("Motion may not have completed as expected. Proceed with caution.");
SerialPort.SendLine();
return false;
} else {
SerialPort.SendLine("Move Done");
return true;
}
}
void HandleMotorFaults(){
SerialPort.SendLine("Handling fault: clearing faults by cycling enable signal to motor.");
motor.EnableRequest(false);
motor.EnableRequest(true);
}