#include "ClearCore.h"
#define INPUT_A_B_FILTER 20
#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);
}
}
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.Send(positionNum);
switch (positionNum) {
case 1:
motor.MotorInAState(false);
motor.MotorInBState(false);
SerialPort.SendLine(" (Inputs A Off/B Off)");
break;
case 2:
motor.MotorInAState(true);
motor.MotorInBState(false);
SerialPort.SendLine(" (Inputs A On/B Off)");
break;
case 3:
motor.MotorInAState(false);
motor.MotorInBState(true);
SerialPort.SendLine(" (Inputs A Off/B On)");
break;
case 4:
motor.MotorInAState(true);
motor.MotorInBState(true);
SerialPort.SendLine(" (Inputs A On/B On)");
break;
default:
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);
}