ClearCore Library
SerialCommExamples/ClearCoreCommandProtocol/ClearCoreCommandProtocol.cpp

Return to SDK Examples for Microchip Studio

/*
* Title: ClearCoreCommandProtocol
*
* Objective:
* This example demonstrates control of various functionality of the Teknic
* ClearCoreI/O and motion controller, including controlling ClearPath-SD
* motors instep and direction mode.
*
* Description:
* This example processes strings of characters formatted according to the
* specifications below and commands the corresponding action on a ClearCore device.
* This example is designed to be highly configurable to meet the requirements
* of a variety of applications. The protocol accepts input and sends output
* via USB connection by default but can be configured to accept commands from
* other streams - such as ClearCore's COM ports, ethernet port, or XBee
* connection - and sources - such as manual user input, input from a text
* file, or control from another device sending text commands to ClearCore.
*
* Setup:
* 0. Consult the accompanying ClearCore Command Protocol User Guide for more
* information on using this example project.
* https://teknic.com/files/downloads/ClearCoreCommandProtocol_UserGuide.pdf
* 1. Connect ClearCore via USB to a terminal that can send and receive ASCII
* (standard text encoding) messages, or modify the code and connect to a
* different source of messages. Other ClearCore examples demonstrate
* communication via these alternate connection options.
* 2. ClearPath-SD motors can be connected to connector M-0, M-1, M-2, and/or M-3.
* 3. The connected ClearPath-SD motor(s) must be configured through the MSP software
* for Step and Direction mode (In MSP select Mode>>Step and Direction).
* 4. The ClearPath-SD motor(s) must be set to use the HLFB mode "ASG-Position
* w/Measured Torque" with a PWM carrier frequency of 482 Hz through the MSP
* software (select Advanced>>High Level Feedback [Mode]... then choose
* "ASG-Position w/Measured Torque" from the dropdown, make sure that 482 Hz
* is selected in the "PWM Carrier Frequency" dropdown, and hit the OK
* button).
* 5. Set the Input Format in MSP for "Step + Direction".
* 6. Set the Input Resolution in MSP the same as your motor's Positioning
* Resolution spec if you'd like the pulses sent by ClearCore to command a
* move of the same number of Encoder Counts, a 1:1 ratio.
* 7. Input and output devices can be wired to connectors IO0-IO5, DI6-DI8, and/or
* AI9-AI12. A table summarizing the acceptable connector modes for each
* ClearCore connector can be found in the ClearCore manual and in the User
* Guide for this example. This example explicitly sets the operational mode
* for each connector, but these modes can be reconfigured according to the
* table in the manual. Be sure to consult the ClearCore manual and
* supplemental wiring and connection diagrams to view the operating modes
* for each pin and corresponding connection setups.
*
* Example Command Sequences
* The following provides an example command sequence and corresponding behavior:
* e1 //enable motor1
* e2 //enable motor2
* m1 1000 // if ABSOLUTE_MOVE==1, move motor1 to absolute position 1000 steps
* // if ABSOLUTE_MOVE==0, move motor1 1000 steps in the positive direction
* v2 -200 //move motor2 at -200 steps/s in the negative direction
* q2s //query the status of motor2
* l3v 100 //limit motor3's velocity (of positional moves only) to 100 steps/s
* z1 //zero Clear's position reference for motor1 (no motion is commanded to the motor)
* i6 //read the current state of connector6 (DI6)
* o5 1 //output a value of 1 (digital high) to connector5 (IO5)
* h //display the help message
*
* The following example command sequence highlights special cases and notable behavior:
* v0 1000 //since motor0 has not yet been enabled, no motion will be commanded
* e0 //enable motor0
* v0 1000 //move motor0 at 1000 steps/s
* d0 //disable motor0. since motor0 was actively moving, motor0 will fault
* v0 1000 //since motor0 is in fault (disabled during motion), no motion will be commanded
* q1s //query the status of motor0. since the motor is in alert, the alert status will also print
* f 0 //disable verbose feedback
* q1s //with verbose feedback disabled, only the numerical status (and alert, in this case)
* // register(s) will print, not the full status (and alert) message
* c0 //clear alerts on motor0
* v0 1000 //move motor0 at 1000 steps/s
*
* m1 2000 //assuming ABSOLUTE_MOVE is left as default, move motor1 to absolute position 2000 steps
* m1 2000 //since motor1 is already at absolute position 2000 steps, no motion will be commanded
* z1 //define motor1's current position as the zero position
* m1 2000 //move motor1 to absolute position 2000, which is 2000 steps
* // away from its freshly-zeroed current position
*
* Additional Resources:
* ** ClearCore Command Protocol User Guide:
* https://teknic.com/files/downloads/ClearCoreCommandProtocol_UserGuide.pdf
* ** ClearCore Documentation:
* https://teknic-inc.github.io/ClearCore-library/
* ** ClearCore Manual:
* https://www.teknic.com/files/downloads/clearcore_user_manual.pdf
* ** ClearCore System Diagram and Connection Diagrams:
* https://www.teknic.com/files/downloads/clearcore_user_manual.pdf
* ** ClearPath Manual (DC Power):
* https://www.teknic.com/files/downloads/clearpath_user_manual.pdf
* ** ClearPath Manual (AC Power):
* https://www.teknic.com/files/downloads/ac_clearpath-mc-sd_manual.pdf
*
* Copyright (c) 2020 Teknic Inc. This work is free to use, copy and distribute under the terms of
* the standard MIT permissive software license which can be found at https://opensource.org/licenses/MIT
*/
#include "ClearCore.h"
#include <stdint.h>
#include <stdlib.h> //for atoi (ASCII to Integer) parsing utility
// specify which serial interface to use as input/output
#define ioPort ConnectorUsb
// select the baud rate to match the target device.
#define ioPortBaudRate 115200
// specify whether the target serial interface uses CTS/RTS flow control
// set to true if your target device uses CTS/RTS flow control
// this is only necessary if using COM ports or XBee module (not necessary for USB)
#define ioFlowControl false
// structure to hold information on the feedback messages
struct FeedbackMessage{
uint32_t number;
char *message;
};
// feedback message numbers
#define FB_COMMAND_OK ( 0)
#define FB_ERR_BUFFER_OVERRUN ( 1)
#define FB_ERR_INPUT_INVALID_NONLETTER ( 2)
#define FB_ERR_MOTOR_NUM_INVALID ( 3)
#define FB_ERR_CONNECTOR_NUM_INVALID ( 4)
#define FB_ERR_CONNECTOR_MODE_INCOMPATIBLE ( 5)
#define FB_ENABLED_WAITING_ON_HLFB ( 6)
#define FB_ENABLE_FAILURE ( 7)
#define FB_ERR_IO_OUTPUT ( 8)
#define FB_ERR_MOVE_NOT_ENABLED ( 9)
#define FB_ERR_MOVE_IN_ALERT (10)
#define FB_ERR_INVALID_QUERY_REQUEST (11)
#define FB_ERR_LIMIT_OUT_OF_BOUNDS (12)
#define FB_ERR_INVALID_LIMIT_REQUEST (13)
#define FB_ERR_INVALID_FEEDBACK_OPTION (14)
#define FB_ERR_UNRECOGNIZED_COMMAND (15)
#define FB_HELP (16)
// verbose feedback messages
char *msg_command_ok =
"Command received";
char *msg_err_buffer_overrun =
"Error: input buffer overrun.";
char *msg_err_input_invalid_nonletter =
"Error: invalid input. Commands begin with a single letter character.";
char *msg_err_motor_num_invalid =
"Error: a required motor was not specified or specified incorrectly. Acceptable motor numbers are 0, 1, 2, and 3.";
char *msg_err_connector_num_invalid =
"Error: a required connector was not specified or specified incorrectly. Acceptable connector numbers are 0 through 12, inclusive.";
char *msg_err_io_output =
"Error: an I/O output parameter is invalid. Ensure the output value is appropriate for the type of output pin.";
char *msg_err_connector_mode_incompatible =
"Error: a specified connector is of an inappropriate mode. Verify the I/O connector is configured as necessary.";
char *msg_enabled_waiting_on_hlfb =
"Motor enabled; waiting on HLFB to assert before accepting other commands.";
char *msg_enable_failure =
"Motor failed to enable due to motor fault, loss of power, or loss/absence of connection. Motor disabled.";
char *msg_err_move_not_enabled =
"Error: motion commanded while motor not enabled. Command e# to enable motor number #.";
char *msg_err_move_in_alert =
"Error: motion commanded while motor in fault. Command c# to clear alerts on motor number #.";
char *msg_err_invalid_query_request =
"Error: invalid query request. Command h for more information.";
char *msg_err_limit_out_of_bounds =
"Error: commanded limit falls outside the acceptable bounds for this limit.";
char *msg_err_invalid_limit_request =
"Error: invalid limit request. Command h for more information.";
char *msg_err_invalid_feedback_option =
"Error: invalid feedback request. Command h for more information.";
char *msg_err_unrecognized_command =
"Error: unrecognized command. Command h for more information.";
char *msg_help =
"ClearCore Command Protocol\n"
"Acceptable commands, where # specifies a motor number* (0, 1, 2, or 3): \n"
" e# | enable specified motor\n"
" d# | disable specified motor\n"
" m# distance | if(ABSOLUTE_MOVE==1) move to the specified position\n"
" if(ABSOLUTE_MOVE==0) move the specified number of steps\n"
" v# velocity | move at the specified velocity (steps/s)\n"
" q#<p/v/s> | query specified motor's position/velocity/status\n"
" l#<v/a> limit | set specified motor's velocity/acceleration limit\n"
" c# | clear alerts\n"
" z# | set the zero position for motor # to the current commanded position\n"
" i# | read input on pin #\n"
" Digital pins return 1 or 0; analog pins return [0,4095] corresponding to [0,10]V\n"
" (*note that # for this command can be 0 through 5)\n"
" o# outputVal | write output on pin #\n"
" Digital pins allow 1 or 0; analog pins allow [409,2047] corresponding to [4,20]mA\n"
" (*note that # for this command can be 0 through 12)\n"
" f fdbkType | specify the type of feedback printed:\n"
" 0 : send message number only\n"
" 1 : send verbose message\n"
" h | print this help message\n";
// array of feedback messages, accessed by SendFeedback() when feedback is sent
FeedbackMessage FeedbackMessages[17] = {
{FB_COMMAND_OK , msg_command_ok },
{FB_ERR_BUFFER_OVERRUN , msg_err_buffer_overrun },
{FB_ERR_INPUT_INVALID_NONLETTER , msg_err_input_invalid_nonletter },
{FB_ERR_MOTOR_NUM_INVALID , msg_err_motor_num_invalid },
{FB_ERR_CONNECTOR_NUM_INVALID , msg_err_connector_num_invalid },
{FB_ERR_CONNECTOR_MODE_INCOMPATIBLE,
msg_err_connector_mode_incompatible },
{FB_ENABLED_WAITING_ON_HLFB , msg_enabled_waiting_on_hlfb },
{FB_ENABLE_FAILURE , msg_enable_failure },
{FB_ERR_IO_OUTPUT , msg_err_io_output },
{FB_ERR_MOVE_NOT_ENABLED , msg_err_move_not_enabled },
{FB_ERR_MOVE_IN_ALERT , msg_err_move_in_alert },
{FB_ERR_INVALID_QUERY_REQUEST , msg_err_invalid_query_request },
{FB_ERR_LIMIT_OUT_OF_BOUNDS , msg_err_limit_out_of_bounds },
{FB_ERR_INVALID_LIMIT_REQUEST , msg_err_invalid_limit_request },
{FB_ERR_INVALID_FEEDBACK_OPTION , msg_err_invalid_feedback_option },
{FB_ERR_UNRECOGNIZED_COMMAND , msg_err_unrecognized_command },
{FB_HELP , msg_help }
};
// global variable to select between printing only feedback number or verbose feedback message
bool verboseFeedback = true;
// macro to select between commanding absolute positional moves or relative
// positional moves. See User Guide for more information.
#define ABSOLUTE_MOVE (1)
// container for the char stream to be read-in.
// allows for IN_BUFFER_LEN characters to be stored followed by a NULL terminator
//todo thoughts on this?
#define IN_BUFFER_LEN 32
char input[IN_BUFFER_LEN+1];
// motor connectors
MotorDriver *const motors[MOTOR_CON_CNT] = {
&ConnectorM0, &ConnectorM1, &ConnectorM2, &ConnectorM3
};
// I/O connectors
Connector *const connectors[13] = {
&ConnectorIO0, &ConnectorIO1, &ConnectorIO2, &ConnectorIO3, &ConnectorIO4, &ConnectorIO5,
&ConnectorDI6, &ConnectorDI7, &ConnectorDI8,
&ConnectorA9, &ConnectorA10, &ConnectorA11, &ConnectorA12
};
// acceleration and velocity limit bounds
// (note that velocity limits take effect only on positional moves)
#define DEFAULT_ACCEL_LIMIT (100000) // pulses per sec^2
#define MAX_ACCEL_LIMIT (1000000000)
#define MIN_ACCEL_LIMIT (1)
#define DEFAULT_VEL_LIMIT (10000) // pulses per sec
#define MAX_VEL_LIMIT (500000)
#define MIN_VEL_LIMIT (1)
// helper functions to print feedback and status information
// the implementations of these functions are at the bottom of this example
void SendFeedback(int32_t messageNumber);
void SendVerboseStatus(int32_t motorNumber);
int main() {
// configure serial communication to USB port and wait for the port to open
ioPort.Mode(Connector::USB_CDC);
ioPort.Speed(ioPortBaudRate);
// ioPort.FlowControl(ioFlowControl); //only necessary if using COM ports or XBee module
ioPort.PortOpen();
while (!ioPort) {
continue;
}
// configure input clocking rate
// this normal rate is ideal for ClearPath step and direction applications
MotorMgr.MotorInputClocking(MotorManager::CLOCK_RATE_NORMAL);
// set all motor connectors into step and direction mode
MotorMgr.MotorModeSet(MotorManager::MOTOR_ALL, Connector::CPM_MODE_STEP_AND_DIR);
// local storage for velocity and acceleration limits
// (note that velocity limits only take effect on positional moves)
uint32_t accelerationLimits[MOTOR_CON_CNT] = {
DEFAULT_ACCEL_LIMIT, DEFAULT_ACCEL_LIMIT, DEFAULT_ACCEL_LIMIT, DEFAULT_ACCEL_LIMIT
};
uint32_t velocityLimits[MOTOR_CON_CNT] = {
DEFAULT_VEL_LIMIT, DEFAULT_VEL_LIMIT, DEFAULT_VEL_LIMIT, DEFAULT_VEL_LIMIT
};
// generic iterator
uint32_t i;
// configure all motor connectors for bipolar PWM HLFB mode at 482Hz, and set
// velocity and acceleration limits
for (i=0; i<MOTOR_CON_CNT; i++){
motors[i]->HlfbMode(MotorDriver::HLFB_MODE_HAS_BIPOLAR_PWM);
motors[i]->HlfbCarrier(MotorDriver::HLFB_CARRIER_482_HZ);
motors[i]->VelMax(velocityLimits[i]);
motors[i]->AccelMax(accelerationLimits[i]);
}
// configure I/O pins
// these defaults can be modified according to application needs
// for more information and to view a list of configurable modes for each pin,
// see the ClearCore manual or the User Guide for this example
ConnectorIO0.Mode(Connector::OUTPUT_ANALOG );
ConnectorIO1.Mode(Connector::OUTPUT_DIGITAL);
ConnectorIO2.Mode(Connector::OUTPUT_DIGITAL);
ConnectorIO3.Mode(Connector::OUTPUT_DIGITAL);
ConnectorIO4.Mode(Connector::OUTPUT_DIGITAL);
ConnectorIO5.Mode(Connector::OUTPUT_DIGITAL);
ConnectorDI6.Mode(Connector::INPUT_DIGITAL );
ConnectorDI7.Mode(Connector::INPUT_DIGITAL );
ConnectorDI8.Mode(Connector::INPUT_DIGITAL );
ConnectorA9 .Mode(Connector::INPUT_ANALOG );
ConnectorA10.Mode(Connector::INPUT_ANALOG );
ConnectorA11.Mode(Connector::INPUT_ANALOG );
ConnectorA12.Mode(Connector::INPUT_ANALOG );
// program control variables
bool inputValid;
bool motorNumValid;
bool connectorNumValid;
bool motorEnabledBeforeClearingAlerts;
// parsing variables
int32_t motorNum_in;
int32_t connectorNum_in;
int32_t moveDistance_in;
int32_t velocity_in;
int32_t limit_in;
int32_t queriedValue;
int16_t inputConnectorValue;
int16_t outputConnectorValue_in;
ioPort.SendLine("Setup successful");
ioPort.SendLine("Send 'h' to receive a list of valid commands");
// main loop to read and process input
while (true) {
// reset the input buffer by populating each index with a NULL character
for (i = 0; i<IN_BUFFER_LEN+1; i++){
input[i] = (char) NULL;
}
// read and store the input character by character
// input buffer has a default maximum size, defined by IN_BUFFER_LEN, of 32.
// if more characters are provided by the user, program will reject input.
inputValid = true;
i = 0;
while(i<IN_BUFFER_LEN && ioPort.CharPeek() != -1){
input[i] = (char) ioPort.CharGet();
i++;
Delay_ms(1);
}
//-------------------------
// This line divides the command reading and storing section of the code from
// the command parsing section of the code. To use this protocol to accept commands
// from another source than the default USB input, ensure that the command is stored
// in a character array called "input" and is ready to be parsed after this line.
//-------------------------
// echo nonempty input when in verbose feedback mode
if(verboseFeedback && i!=0){
ioPort.SendLine(input);
}
if (i==0){
// user did not input any characters. input invalid, but no need to report
inputValid = false;
} else if (ioPort.CharPeek()!=-1){
// buffer overflow (there is no space left in the buffer, but there are still characters to read from ioPort)
// report error, reject input, and flush input stream (so the leftover characters aren't read as part of the next command)
SendFeedback(FB_ERR_BUFFER_OVERRUN);
inputValid = false;
while(ioPort.CharPeek()!=-1){
ioPort.FlushInput();
Delay_ms(10);
}
}
// verify first character of command is a letter
if (inputValid){
if (('A' <= input[0] && input[0] <= 'Z') || ('a' <= input[0] && input[0] <= 'z')){
// if letter is uppercase, convert to lowercase
if ('A' <= input[0] && input[0] <= 'Z'){
// to convert an ASCII character from upper to lower case, add 32 to the character
input[0] += 32;
}
} else {
// if first letter of command is not letter, reject input
SendFeedback(FB_ERR_INPUT_INVALID_NONLETTER);
inputValid = false;
ioPort.FlushInput();
}
}
if (inputValid){
// store and validate motor number input from command
motorNum_in = atoi(&input[1]);
motorNumValid = (0<=motorNum_in && motorNum_in<=3 && input[1]!=NULL);
// store and validate connector number input from command
connectorNum_in = atoi(&input[1]);
connectorNumValid = (0<=connectorNum_in && connectorNum_in<=12 && input[1]!=NULL);
// status register for accessing motor status information
volatile const MotorDriver::StatusRegMotor &statusReg = motors[motorNum_in]->StatusReg();
volatile const MotorDriver::AlertRegMotor &alertReg = motors[motorNum_in]->AlertReg();
// process the command based on the command letter (first character of input)
switch(input[0]){
// enable
case 'e':
// verify motor number valid
if (!motorNumValid){
SendFeedback(FB_ERR_MOTOR_NUM_INVALID);
} else {
// enable the motor
motors[motorNum_in]->EnableRequest(true);
SendFeedback(FB_ENABLED_WAITING_ON_HLFB);
// wait until motor is ready before accepting other commands
// (allows any automatic homing move to complete if configured)
// (loop will exit on fault during homing, or if motor is disconnected or loses power)
while (statusReg.bit.HlfbState != MotorDriver::HLFB_ASSERTED &&
!statusReg.bit.MotorInFault){
continue;
}
if(statusReg.bit.MotorInFault){
// if there is a fault while trying to enable, disable and report
motors[motorNum_in]->EnableRequest(false);
SendFeedback(FB_ENABLE_FAILURE);
} else {
SendFeedback(FB_COMMAND_OK);
}
}
break;
// disable
case 'd':
// verify motor number valid
if (!motorNumValid){
SendFeedback(FB_ERR_MOTOR_NUM_INVALID);
} else {
// disable the motor
motors[motorNum_in]->EnableRequest(false);
SendFeedback(FB_COMMAND_OK);
}
break;
// position move
case 'm':
// verify motor number valid, motor enabled, and no faults present
if (!motorNumValid){
SendFeedback(FB_ERR_MOTOR_NUM_INVALID);
} else if ( statusReg.bit.ReadyState==MotorDriver::MOTOR_DISABLED ||
statusReg.bit.ReadyState==MotorDriver::MOTOR_ENABLING){
SendFeedback(FB_ERR_MOVE_NOT_ENABLED);
} else if (motors[motorNum_in]->StatusReg().bit.AlertsPresent) {
SendFeedback(FB_ERR_MOVE_IN_ALERT);
} else {
// command the move
// #define ABSOLUTE_MOVE (0) commands absolute moves.
// #define ABSOLUTE_MOVE (1) commands relative moves.
// absolute moves are configured by default.
// See User Guide for more information.
moveDistance_in = atoi(&input[2]);
if (ABSOLUTE_MOVE){
motors[motorNum_in]->Move(moveDistance_in, MotorDriver::MOVE_TARGET_ABSOLUTE);
} else {
motors[motorNum_in]->Move(moveDistance_in);
}
SendFeedback(FB_COMMAND_OK);
}
break;
// velocity move
case 'v':
// verify motor number valid, motor enabled, and no faults present
if (!motorNumValid){
SendFeedback(FB_ERR_MOTOR_NUM_INVALID);
} else if ( statusReg.bit.ReadyState==MotorDriver::MOTOR_DISABLED ||
statusReg.bit.ReadyState==MotorDriver::MOTOR_ENABLING){
SendFeedback(FB_ERR_MOVE_NOT_ENABLED);
} else if (motors[motorNum_in]->StatusReg().bit.AlertsPresent) {
SendFeedback(FB_ERR_MOVE_IN_ALERT);
} else {
// command the move
velocity_in = atoi(&input[2]);
motors[motorNum_in]->MoveVelocity(velocity_in);
SendFeedback(FB_COMMAND_OK);
}
break;
// query position, velocity, or status
case 'q':
// verify motor number valid
if (!motorNumValid){
SendFeedback(FB_ERR_MOTOR_NUM_INVALID);
break;
}
switch(input[2]){
// query commanded position
case 'p':
case 'P':
// send commanded position.
// this can differ from the position counter in MSP if the ClearCore
// position reference has not been synced with ClearPath's position
queriedValue = (int32_t) motors[motorNum_in]->PositionRefCommanded();
if (verboseFeedback){
ioPort.Send("Motor ");
ioPort.Send(motorNum_in);
ioPort.Send(" is in position (steps) ");
}
ioPort.SendLine(queriedValue);
break;
// query velocity
case 'v':
case 'V':
// send motor velocity
queriedValue = motors[motorNum_in]->VelocityRefCommanded();
if (verboseFeedback){
ioPort.Send("Motor ");
ioPort.Send(motorNum_in);
ioPort.Send(" is at velocity (steps/s) ");
}
ioPort.SendLine(queriedValue);
break;
// query status
case 's':
case 'S':
// send motor status
if (verboseFeedback){
SendVerboseStatus(motorNum_in);
} else {
ioPort.SendLine(statusReg.reg, 2); // prints the status register in binary
if (statusReg.bit.AlertsPresent){
ioPort.SendLine(alertReg.reg, 2); // prints the alert register in binary
}
}
break;
// invalid query request
default:
SendFeedback(FB_ERR_INVALID_QUERY_REQUEST);
}
break;
// set acceleration or velocity limit
case 'l':
// verify motor number valid
if ( !motorNumValid){
SendFeedback(FB_ERR_MOTOR_NUM_INVALID);
break;
}
// store limit input from command
limit_in = atoi(&input[3]);
switch(input[2]){
// velocity limit
case 'v':
case 'V':
//verify limit is valid, store, then propagate the change to the motor
if (MIN_VEL_LIMIT<=limit_in && limit_in<=MAX_VEL_LIMIT){
velocityLimits[motorNum_in] = limit_in;
motors[motorNum_in]->VelMax(velocityLimits[motorNum_in]);
SendFeedback(FB_COMMAND_OK);
} else {
SendFeedback(FB_ERR_LIMIT_OUT_OF_BOUNDS);
}
break;
// acceleration limit
case 'a':
case 'A':
//verify limit is valid, store, then propagate the change to the motor
if (MIN_ACCEL_LIMIT<=limit_in && limit_in<=MAX_ACCEL_LIMIT){
accelerationLimits[motorNum_in] = limit_in;
motors[motorNum_in]->AccelMax(velocityLimits[motorNum_in]);
SendFeedback(FB_COMMAND_OK);
} else {
SendFeedback(FB_ERR_LIMIT_OUT_OF_BOUNDS);
}
break;
// invalid limit request
default:
SendFeedback(FB_ERR_INVALID_LIMIT_REQUEST);
break;
}
break;
// clear alerts
case 'c':
// verify motor number valid
if (!motorNumValid){
SendFeedback(FB_ERR_MOTOR_NUM_INVALID);
break;
}
// capture the current state of enable
// this value will be restored after alerts are cleared
motorEnabledBeforeClearingAlerts = motors[motorNum_in]->EnableRequest();
// to clear all ClearCore alerts (which can include motor faults):
// - cycle enable if faults present (clears faults, if any)
// - clear alert register (clears alerts)
// this command clears both ClearCore motor alerts and motor faults
if (statusReg.bit.MotorInFault){
motors[motorNum_in]->EnableRequest(false);
Delay_ms(10);
if(motorEnabledBeforeClearingAlerts){
motors[motorNum_in]->EnableRequest(true);
}
}
motors[motorNum_in]->ClearAlerts();
SendFeedback(FB_COMMAND_OK);
break;
// set the zero position for motor # to the current commanded position
case 'z':
// verify motor number valid
if (!motorNumValid){
SendFeedback(FB_ERR_MOTOR_NUM_INVALID);
break;
}
// zero position
motors[motorNum_in]->PositionRefSet(0);
SendFeedback(FB_COMMAND_OK);
break;
// read input from ClearCore connector
case 'i':
// verify connector number valid
if (!connectorNumValid){
SendFeedback(FB_ERR_CONNECTOR_NUM_INVALID);
break;
}
// verify connector mode valid
if (connectors[connectorNum_in]->Mode() != Connector::INPUT_DIGITAL &&
connectors[connectorNum_in]->Mode() != Connector::INPUT_ANALOG){
SendFeedback(FB_ERR_CONNECTOR_MODE_INCOMPATIBLE);
break;
}
inputConnectorValue = connectors[connectorNum_in]->State();
if(verboseFeedback){
ioPort.Send("Connector ");
ioPort.Send(connectorNum_in);
ioPort.Send(" value: ");
}
ioPort.SendLine(inputConnectorValue);
break;
// write digital output to ClearCore digital output connector
case 'o':
// verify connector number valid
if (!connectorNumValid){
SendFeedback(FB_ERR_CONNECTOR_NUM_INVALID);
break;
}
// verify connector type valid
if (connectors[connectorNum_in]->Mode() != Connector::OUTPUT_DIGITAL &&
connectors[connectorNum_in]->Mode() != Connector::OUTPUT_ANALOG){
SendFeedback(FB_ERR_CONNECTOR_MODE_INCOMPATIBLE);
break;
}
// store and write value
outputConnectorValue_in = atoi(&input[3]);
if(!connectors[connectorNum_in]->State(outputConnectorValue_in)){
SendFeedback(FB_ERR_IO_OUTPUT);
} else {
SendFeedback(FB_COMMAND_OK);
}
break;
// change feedback type
case 'f':
switch(atoi(&input[1])){
// feedback number only
case 0:
verboseFeedback = false;
SendFeedback(FB_COMMAND_OK);
break;
// verbose feedback messages
case 1:
verboseFeedback = true;
SendFeedback(FB_COMMAND_OK);
break;
// invalid feedback request
default:
SendFeedback(FB_ERR_INVALID_FEEDBACK_OPTION);
break;
}
break;
// help
case 'h':
SendFeedback(FB_HELP);
break;
// invalid command letter
default:
SendFeedback(FB_ERR_UNRECOGNIZED_COMMAND);
break;
} // switch(command letter)
} // if (inputValid)
} // while(true)
} // main()
/*------------------------------------------------------------------------------
* SendFeedback
*
* Helper function to send feedback for specified feedback message.
*
* Parameters:
* int messageNumber - The feedback identification number corresponding the
* feedback struct array index
*
* Returns: None (feedback message is sent directly to output)
*/
void SendFeedback(int32_t messageNumber){
// send either the verbose message or only the message number, based on verboseFeedback bool
if (verboseFeedback){
ioPort.SendLine(FeedbackMessages[messageNumber].message);
} else {
ioPort.SendLine(FeedbackMessages[messageNumber].number);
}
} // SendFeedback()
//------------------------------------------------------------------------------
/*------------------------------------------------------------------------------
* SendVerboseStatus
*
* Outputs verbose status information.
* Functionality adapted from MotorStatusRegister example project
*
* Parameters:
* int motorNumber - The motor whose status should be printed
*
* Returns: None (status message is sent directly to output)
*/
void SendVerboseStatus(int32_t motorNumber) {
// status register for accessing motor status information
volatile const MotorDriver::StatusRegMotor &statusReg = motors[motorNumber]->StatusReg();
volatile const MotorDriver::AlertRegMotor &alertReg = motors[motorNumber]->AlertReg();
ioPort.Send("Motor status register for motor M");
ioPort.Send(motorNumber);
ioPort.Send(": ");
ioPort.SendLine(statusReg.reg, 2); // prints the status register in binary
ioPort.Send("AtTargetPosition: ");
ioPort.SendLine(statusReg.bit.AtTargetPosition);
ioPort.Send("StepsActive: ");
ioPort.SendLine(statusReg.bit.StepsActive);
ioPort.Send("AtTargetVelocity: ");
ioPort.SendLine(statusReg.bit.AtTargetVelocity);
ioPort.Send("MoveDirection: ");
ioPort.SendLine(statusReg.bit.MoveDirection);
ioPort.Send("MotorInFault: ");
ioPort.SendLine(statusReg.bit.MotorInFault);
ioPort.Send("Enabled: ");
ioPort.SendLine(statusReg.bit.Enabled);
ioPort.Send("PositionalMove: ");
ioPort.SendLine(statusReg.bit.PositionalMove);
ioPort.Send("HLFB State: ");
switch (statusReg.bit.HlfbState) {
case 0:
ioPort.SendLine("HLFB_DEASSERTED");
break;
case 1:
ioPort.SendLine("HLFB_ASSERTED");
break;
case 2:
ioPort.SendLine("HLFB_HAS_MEASUREMENT");
break;
case 3:
ioPort.SendLine("HLFB_UNKNOWN");
break;
default:
// something has gone wrong if this is printed
ioPort.SendLine("???");
}
ioPort.Send("AlertsPresent: ");
ioPort.SendLine(statusReg.bit.AlertsPresent);
ioPort.Send("Ready state: ");
switch (statusReg.bit.ReadyState) {
case MotorDriver::MOTOR_DISABLED:
ioPort.SendLine("Disabled");
break;
case MotorDriver::MOTOR_ENABLING:
ioPort.SendLine("Enabling");
break;
case MotorDriver::MOTOR_FAULTED:
ioPort.SendLine("Faulted");
break;
case MotorDriver::MOTOR_READY:
ioPort.SendLine("Ready");
break;
case MotorDriver::MOTOR_MOVING:
ioPort.SendLine("Moving");
break;
default:
// something has gone wrong if this is printed
ioPort.SendLine("???");
}
ioPort.Send("Triggering: ");
ioPort.SendLine(statusReg.bit.Triggering);
ioPort.Send("InPositiveLimit: ");
ioPort.SendLine(statusReg.bit.InPositiveLimit);
ioPort.Send("InNegativeLimit: ");
ioPort.SendLine(statusReg.bit.InNegativeLimit);
ioPort.Send("InEStopSensor: ");
ioPort.SendLine(statusReg.bit.InEStopSensor);
ioPort.SendLine("--------------------------------");
if (statusReg.bit.AlertsPresent){
ioPort.Send("Alert register: ");
ioPort.SendLine(alertReg.reg, 2); // prints the alert register in binary
ioPort.Send("MotionCanceledInAlert: ");
ioPort.SendLine(alertReg.bit.MotionCanceledInAlert);
ioPort.Send("MotionCanceledPositiveLimit: ");
ioPort.SendLine(alertReg.bit.MotionCanceledPositiveLimit);
ioPort.Send("MotionCanceledNegativeLimit: ");
ioPort.SendLine(alertReg.bit.MotionCanceledNegativeLimit);
ioPort.Send("MotionCanceledSensorEStop: ");
ioPort.SendLine(alertReg.bit.MotionCanceledSensorEStop);
ioPort.Send("MotionCanceledMotorDisabled: ");
ioPort.SendLine(alertReg.bit.MotionCanceledMotorDisabled);
ioPort.Send("MotorFaulted: ");
ioPort.SendLine(alertReg.bit.MotorFaulted);
ioPort.SendLine("--------------------------------");
}
}// SendVerboseStatus()
//------------------------------------------------------------------------------
Generated by   doxygen 1.8.11