#include "ClearCore.h"
#define baudRate 9600
#define SerialPort ConnectorUsb
};
char motorConnectorNames[][4] = { "M-0", "M-1", "M-2", "M-3" };
uint8_t motorConnectorCount = 4;
char *readyStateStr;
char *ReadyStateString(MotorDriver::MotorReadyStates readyState);
int main() {
SerialPort.Mode(Connector::USB_CDC);
SerialPort.Speed(baudRate);
uint32_t timeout = 5000;
SerialPort.PortOpen();
while (!SerialPort &&
Milliseconds() - startTime < timeout) {
continue;
}
while (true) {
for (uint8_t i = 0; i < motorConnectorCount; i++) {
MotorDriver *motor = motorConnectors[i];
volatile const MotorDriver::StatusRegMotor &statusReg = motor->StatusReg();
volatile const MotorDriver::AlertRegMotor &alertReg = motor->AlertReg();
SerialPort.Send("Motor status register for motor M");
SerialPort.Send(i);
SerialPort.Send(": ");
SerialPort.SendLine(statusReg.reg, 2);
SerialPort.Send("AtTargetPosition: ");
SerialPort.SendLine(statusReg.bit.AtTargetPosition);
SerialPort.Send("StepsActive: ");
SerialPort.SendLine(statusReg.bit.StepsActive);
SerialPort.Send("AtTargetVelocity: ");
SerialPort.SendLine(statusReg.bit.AtTargetVelocity);
SerialPort.Send("MoveDirection: ");
SerialPort.SendLine(statusReg.bit.MoveDirection);
SerialPort.Send("MotorInFault: ");
SerialPort.SendLine(statusReg.bit.MotorInFault);
SerialPort.Send("Enabled: ");
SerialPort.SendLine(statusReg.bit.Enabled);
SerialPort.Send("PositionalMove: ");
SerialPort.SendLine(statusReg.bit.PositionalMove);
SerialPort.Send("HLFB State: ");
switch (statusReg.bit.HlfbState) {
case 0:
SerialPort.SendLine("HLFB_DEASSERTED");
break;
case 1:
SerialPort.SendLine("HLFB_ASSERTED");
break;
case 2:
SerialPort.SendLine("HLFB_HAS_MEASUREMENT");
break;
case 3:
SerialPort.SendLine("HLFB_UNKNOWN");
break;
default:
SerialPort.SendLine("???");
}
SerialPort.Send("AlertsPresent: ");
SerialPort.SendLine(statusReg.bit.AlertsPresent);
SerialPort.Send("Ready state: ");
switch (statusReg.bit.ReadyState) {
case MotorDriver::MOTOR_DISABLED:
SerialPort.SendLine("Disabled");
break;
case MotorDriver::MOTOR_ENABLING:
SerialPort.SendLine("Enabling");
break;
case MotorDriver::MOTOR_FAULTED:
SerialPort.SendLine("Faulted");
break;
case MotorDriver::MOTOR_READY:
SerialPort.SendLine("Ready");
break;
case MotorDriver::MOTOR_MOVING:
SerialPort.SendLine("Moving");
break;
default:
SerialPort.SendLine("???");
}
SerialPort.Send("Triggering: ");
SerialPort.SendLine(statusReg.bit.Triggering);
SerialPort.Send("InPositiveLimit: ");
SerialPort.SendLine(statusReg.bit.InPositiveLimit);
SerialPort.Send("InNegativeLimit: ");
SerialPort.SendLine(statusReg.bit.InNegativeLimit);
SerialPort.Send("InEStopSensor: ");
SerialPort.SendLine(statusReg.bit.InEStopSensor);
SerialPort.SendLine("--------------------------------");
if (statusReg.bit.AlertsPresent){
SerialPort.Send("Alert register: ");
SerialPort.SendLine(alertReg.reg, 2);
SerialPort.Send("MotionCanceledInAlert: ");
SerialPort.SendLine(alertReg.bit.MotionCanceledInAlert);
SerialPort.Send("MotionCanceledPositiveLimit: ");
SerialPort.SendLine(alertReg.bit.MotionCanceledPositiveLimit);
SerialPort.Send("MotionCanceledNegativeLimit: ");
SerialPort.SendLine(alertReg.bit.MotionCanceledNegativeLimit);
SerialPort.Send("MotionCanceledSensorEStop: ");
SerialPort.SendLine(alertReg.bit.MotionCanceledSensorEStop);
SerialPort.Send("MotionCanceledMotorDisabled: ");
SerialPort.SendLine(alertReg.bit.MotionCanceledMotorDisabled);
SerialPort.Send("MotorFaulted: ");
SerialPort.SendLine(alertReg.bit.MotorFaulted);
SerialPort.SendLine("--------------------------------");
}
}
}
}
char *ReadyStateString(MotorDriver::MotorReadyStates readyState) {
switch (readyState) {
case MotorDriver::MOTOR_DISABLED:
return (char *)"Disabled";
case MotorDriver::MOTOR_ENABLING:
return (char *)"Enabling";
case MotorDriver::MOTOR_FAULTED:
return (char *)"Faulted";
case MotorDriver::MOTOR_READY:
return (char *)"Ready";
case MotorDriver::MOTOR_MOVING:
return (char *)"Moving";
default:
return (char *)"???";
}
}