#include "ClearCore.h"
#define motor ConnectorM0
#define AnalogSensor ConnectorA9
#define INPUT_A_FILTER 20
#define baudRate 9600
#define SerialPort ConnectorUsb
double maxSpeed = 510;
bool CommandVelocity(int32_t commandedVelocity);
int main() {
AnalogSensor.Mode(Connector::INPUT_ANALOG);
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");
while (true) {
float analogVoltage = AnalogSensor.AnalogVoltage();
int32_t commandedVelocity =
static_cast<int32_t>(round(analogVoltage / 10 * maxSpeed));
CommandVelocity(commandedVelocity);
}
}
bool CommandVelocity(int32_t commandedVelocity) {
if (abs(commandedVelocity) >= abs(maxSpeed)) {
SerialPort.SendLine("Move rejected, requested velocity at or over the limit.");
return false;
}
if (motor.StatusReg().bit.AlertsPresent) {
SerialPort.SendLine("Motor status: 'In Alert'. Move Canceled.");
return false;
}
SerialPort.Send("Commanding velocity: ");
SerialPort.SendLine(commandedVelocity);
if (commandedVelocity >= 0) {
motor.MotorInAState(false);
}
else {
motor.MotorInAState(true);
}
double scaleFactor = 255 / maxSpeed;
uint8_t dutyRequest = abs(commandedVelocity) * scaleFactor;
motor.MotorInBDuty(dutyRequest);
return true;
}