ClearCore Library
ClearPathModeExamples/ClearPath-MC_Series/Abs4PositionsHomeToHardStop/Abs4PositionsHomeToHardStop.cpp

Return to SDK Examples for Microchip Studio

/*
* Title: 4PositionsHomeToHardStop
*
* Objective:
* This example demonstrates control of the ClearPath-MCPV operational mode
* Move To Absolute Position, 4 Positions (Home to Hard Stop).
*
* Description:
* This example enables, homes, and then moves a ClearPath motor between
* preprogrammed absolute positions as defined in the MSP software. During
* operation, various move statuses are written to the USB serial port.
*
* Requirements:
* 1. A ClearPath motor must be connected to Connector M-0.
* 2. The connected ClearPath motor must be configured through the MSP software
* for Move To Absolute Position, 4 Positions (Home to Hard Stop) mode (In
* MSP select Mode>>Position>>Move to Absolute Position, then with "4
* Positions (Home to Hard Stop)" selected hit the OK button).
* 3. The ClearPath motor 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).
* 4. The ClearPath must have defined Absolute Position Selections through
* the MSP software (On the main MSP window check the "Position Selection
* Setup (cnts)" box and fill in the four text boxes labeled "A off B off",
* "A on B off", "A off B on" and "A on B on").
* 5. Homing must be configured in the MSP software for your mechanical
* system (e.g. homing direction, torque limit, etc.). To configure, click
* the "Setup..." button found under the "Homing" label on the MSP's main
* window.
* 6. Ensure the Input A & B filters in MSP are both set to 20ms (In MSP
* select Advanced>>Input A, B Filtering... then in the Settings box fill in
* the textboxes labeled "Input A Filter Time Constant (msec)" and "Input B
* Filter Time Constant (msec)" then hit the OK button).
*
*
* Links:
* ** ClearCore Documentation: https://teknic-inc.github.io/ClearCore-library/
* ** ClearCore Manual: 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
* ** ClearPath Mode Informational Video: https://www.teknic.com/watch-video/#OpMode3
*
*
* 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"
// The INPUT_A_B_FILTER must match the Input A, B filter setting in
// MSP (Advanced >> Input A, B Filtering...)
#define INPUT_A_B_FILTER 20
// Defines the motor's connector as ConnectorM0
#define motor ConnectorM0
// Select the baud rate to match the target device.
#define baudRate 9600
// Specify which serial to use: ConnectorUsb, ConnectorCOM0, or ConnectorCOM1.
#define SerialPort ConnectorUsb
// This example has built-in functionality to automatically clear motor faults.
// Any uncleared fault will cancel and disallow motion.
// WARNING: enabling automatic fault handling will clear faults immediately when
// encountered and return a motor to a state in which motion is allowed. Before
// enabling this functionality, be sure to understand this behavior and ensure
// your system will not enter an unsafe state.
// To enable automatic fault handling, #define HANDLE_MOTOR_FAULTS (1)
// To disable automatic fault handling, #define HANDLE_MOTOR_FAULTS (0)
#define HANDLE_MOTOR_FAULTS (0)
// Declares user-defined helper functions.
// The definition/implementations of these functions are at the bottom of the sketch.
bool MoveToPosition(uint8_t positionNum);
void HandleMotorFaults();
int main() {
// Sets all motor connectors to the correct mode for Absolute Position mode
MotorMgr.MotorModeSet(MotorManager::MOTOR_ALL,
Connector::CPM_MODE_A_DIRECT_B_DIRECT);
// Set the motor's HLFB mode to bipolar PWM
motor.HlfbMode(MotorDriver::HLFB_MODE_HAS_BIPOLAR_PWM);
// Set the HFLB carrier frequency to 482 Hz
motor.HlfbCarrier(MotorDriver::HLFB_CARRIER_482_HZ);
// Enforces the state of the motor's A and B inputs before enabling
// the motor.
motor.MotorInAState(false);
motor.MotorInBState(false);
// Sets up serial communication and waits up to 5 seconds for a port to open.
// Serial communication is not required for this example to run.
SerialPort.Mode(Connector::USB_CDC);
SerialPort.Speed(baudRate);
uint32_t timeout = 5000;
uint32_t startTime = Milliseconds();
SerialPort.PortOpen();
while (!SerialPort && Milliseconds() - startTime < timeout) {
continue;
}
// Enables the motor; homing will begin automatically
motor.EnableRequest(true);
SerialPort.SendLine("Motor Enabled");
// Waits for HLFB to assert (waits for homing to complete if applicable)
SerialPort.SendLine("Waiting for HLFB...");
while (motor.HlfbState() != MotorDriver::HLFB_ASSERTED &&
!motor.StatusReg().bit.MotorInFault) {
continue;
}
// Check if a motor faulted during enabling
// Clear fault if configured to do so
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) {
// Move to Position 1 defined in MSP.
MoveToPosition(1); // See below for the detailed function definition.
// Wait 1000ms.
Delay_ms(1000);
MoveToPosition(2);
Delay_ms(1000);
MoveToPosition(3);
Delay_ms(1000);
MoveToPosition(4);
Delay_ms(1000);
// Alternatively, if you'd like to control the ClearPath servo's inputs
// externally using ClearCore inputs, consider doing something like this:
/*
// Sets ClearPath's InA to DI6's state
motor.MotorInAState(ConnectorDI6.State());
// Sets ClearPath's InB to DI7's state
motor.MotorInBState(ConnectorDI7.State());
*/
}
}
/*------------------------------------------------------------------------------
* MoveToPosition
*
* Move to position number positionNum (defined in MSP)
* Prints the move status to the USB serial port
* Returns when HLFB asserts (indicating the motor has reached the commanded
* position)
*
* Parameters:
* int positionNum - The position number to command (defined in MSP)
*
* Returns: True/False depending on whether the position was successfully
* commanded.
*/
bool MoveToPosition(uint8_t positionNum) {
// Check if a motor fault is currently preventing motion
// Clear fault if configured to do so
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:
// Sets Input A and B for position 1
motor.MotorInAState(false);
motor.MotorInBState(false);
SerialPort.SendLine(" (Inputs A Off/B Off)");
break;
case 2:
// Sets Input A and B for position 2
motor.MotorInAState(true);
motor.MotorInBState(false);
SerialPort.SendLine(" (Inputs A On/B Off)");
break;
case 3:
// Sets Input A and B for position 3
motor.MotorInAState(false);
motor.MotorInBState(true);
SerialPort.SendLine(" (Inputs A Off/B On)");
break;
case 4:
// Sets Input A and B for position 4
motor.MotorInAState(true);
motor.MotorInBState(true);
SerialPort.SendLine(" (Inputs A On/B On)");
break;
default:
// If this case is reached then an incorrect positionNum was entered
return false;
}
// Ensures this delay is at least 2ms longer than the Input A, B filter
// setting in MSP
Delay_ms(2 + INPUT_A_B_FILTER);
// Waits for HLFB to assert (signaling the move has successfully completed)
SerialPort.SendLine("Moving.. Waiting for HLFB");
while (motor.HlfbState() != MotorDriver::HLFB_ASSERTED &&
!motor.StatusReg().bit.MotorInFault) {
continue;
}
// Check if a motor faulted during move
// Clear fault if configured to do so
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;
}
}
//------------------------------------------------------------------------------
/*------------------------------------------------------------------------------
* HandleMotorFaults
*
* Clears motor faults by cycling enable to the motor.
* Assumes motor is in fault
* (this function is called when motor.StatusReg.MotorInFault == true)
*
* Parameters:
* requires "motor" to be defined as a ClearCore motor connector
*
* Returns:
* none
*/
void HandleMotorFaults(){
SerialPort.SendLine("Handling fault: clearing faults by cycling enable signal to motor.");
motor.EnableRequest(false);
Delay_ms(10);
motor.EnableRequest(true);
Delay_ms(100);
}
//------------------------------------------------------------------------------