#include "ClearCore.h"
#define INPUT_A_FILTER 20
#define motor ConnectorM0
#define baudRate 9600
#define SerialPort ConnectorUsb
double maxTorque = 100;
bool CommandTorque(int8_t commandedTorque);
int main() {
Connector::CPM_MODE_A_DIRECT_B_PWM);
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) {
continue;
}
SerialPort.SendLine("Motor Ready");
while (true) {
CommandTorque(15);
CommandTorque(-75);
CommandTorque(5);
CommandTorque(-35);
CommandTorque(10);
}
}
bool CommandTorque(int8_t commandedTorque) {
if (abs(commandedTorque) > abs(maxTorque)) {
SerialPort.SendLine("Move rejected, invalid torque requested");
return false;
}
if (motor.StatusReg().bit.AlertsPresent) {
SerialPort.SendLine("Motor status: 'In Alert'. Move Canceled.");
return false;
}
SerialPort.Send("Commanding torque: ");
SerialPort.SendLine(commandedTorque);
double scaleFactor = 255 / maxTorque;
uint8_t dutyRequest = abs(commandedTorque) * scaleFactor;
if (commandedTorque < 0) {
motor.MotorInAState(true);
}
else {
motor.MotorInAState(false);
}
motor.MotorInBDuty(dutyRequest);
SerialPort.SendLine("Moving... Waiting for HLFB");
while (motor.HlfbState() != MotorDriver::HLFB_ASSERTED) {
continue;
}
SerialPort.SendLine("Move Done");
return true;
}