FollowDigitalTorque.cpp

Return to SDK Examples for Microchip Studio

/*
 * Title: FollowDigitalTorque
 *
 * Objective:
 *    This example demonstrates control of the ClearPath-MC operational mode
 *    Follow Digital Torque Command, Unipolar PWM Command.
 *
 * Description:
 *    This example enables and then commands a ClearPath motor to output various
 *    torques. in both the clockwise and counterclockwise directions.
 *    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 Follow Digital Torque Command, Unipolar PWM Command mode  (In MSP
 *    select Mode>>Torque>>Follow Digital Torque Command, then with
 *    "Unipolar PWM Command" selected hit the OK button).
 * 3. The ClearPath motor must be set to use the HLFB mode "ASG-Torque"
 *    through the MSP software (select Advanced>>High Level Feedback [Mode]...
 *    then choose "All Systems Go (ASG) - Torque" from the dropdown and hit
 *    the OK button).
 * 4. The ClearPath must have a defined Max Torque Command configured through
 *    the MSP software. This value must match the "maxTorque" variable defined
 *    below.
 * 5. Ensure the Input A filter in MSP is set to 20ms (In MSP select
 *    Advanced>>Input A, B Filtering... then in the Settings box fill in the
 *    textbox labeled "Input A Filter Time Constant (msec)" then hit the OK
 *    button).
 * 6. Set your Max Speed and Over-Speed Timeout in MSP according to your
 *    mechanical system. Note you may notice any of the following if this max
 *    speed is reached: motor shutdown, speed limit cycling at the max speed,
 *    HLFB not signaling ASG/move done.
 * 7. Ensure the "Invert PWM Input" box is unchecked in MSP.
 *
 * 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/#OpMode9
 *
 * 
 * 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_FILTER must match the Input A filter setting in
// MSP (Advanced >> Input A, B Filtering...)
#define INPUT_A_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
 
// Defines the limit of the torque command, as a percent of the motor's peak
// torque rating (must match the value used in MSP)
double maxTorque = 100;
 
// Declares our user-defined helper function, which is used to command torque.
// The definition/implementation of this function is at the bottom of the sketch.
bool CommandTorque(int8_t commandedTorque);
 
int main() {
    // Sets all motor connectors to the correct mode for Follow Digital
    // Torque mode.
    MotorMgr.MotorModeSet(MotorManager::MOTOR_ALL,
                          Connector::CPM_MODE_A_DIRECT_B_PWM);
 
    // 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
    motor.EnableRequest(true);
    SerialPort.SendLine("Motor Enabled");
 
    // Waits for HLFB to assert
    SerialPort.SendLine("Waiting for HLFB...");
    while (motor.HlfbState() != MotorDriver::HLFB_ASSERTED) {
        continue;
    }
    SerialPort.SendLine("Motor Ready");
 
    while (true) {
        // Output 15% of the motor's peak torque in the positive (CCW) direction.
        CommandTorque(15);    // See below for the detailed function definition.
        // Wait 2000ms.
        Delay_ms(2000);
 
        CommandTorque(-75); // Output 75% peak torque in the negative (CW) direction.
        Delay_ms(2000);
 
        CommandTorque(5); // Output 5% peak torque in the positive (CCW) direction.
        Delay_ms(2000);
 
        CommandTorque(-35); // Output 35% peak torque in the negative (CW) direction.
        Delay_ms(2000);
 
        CommandTorque(10); // Output 10% peak torque in the positive (CCW) direction.
        Delay_ms(2000);
    }
}
 
/*------------------------------------------------------------------------------
 * CommandTorque
 *
 *    Command the motor to move using a torque of commandedTorque
 *    Prints the move status to the USB serial port
 *    Returns when HLFB asserts (indicating the motor has reached the commanded
 *    torque)
 *
 * Parameters:
 *    int commandedTorque  - The torque to command
 *
 * Returns: True/False depending on whether the torque was successfully
 *    commanded.
 */
bool CommandTorque(int8_t commandedTorque) {
    if (abs(commandedTorque) > abs(maxTorque)) {
        SerialPort.SendLine("Move rejected, invalid torque requested");
        return false;
    }
 
    // Check if an alert is currently preventing motion
    if (motor.StatusReg().bit.AlertsPresent) {
        SerialPort.SendLine("Motor status: 'In Alert'. Move Canceled.");
        return false;
    }
 
    SerialPort.Send("Commanding torque: ");
    SerialPort.SendLine(commandedTorque);
 
    // Find the scaling factor of our torque range mapped to the PWM duty cycle
    // range (255 is the max duty cycle).
    double scaleFactor = 255 / maxTorque;
 
    // Scale the torque command to our duty cycle range.
    uint8_t dutyRequest = abs(commandedTorque) * scaleFactor;
 
    // Set input A to match the direction of torque.
    if (commandedTorque < 0) {
        motor.MotorInAState(true);
    }
    else {
        motor.MotorInAState(false);
    }
 
    // Ensures this delay is at least 20ms longer than the Input A filter
    // setting in MSP
    Delay_ms(20 + INPUT_A_FILTER);
 
    // Command the move
    motor.MotorInBDuty(dutyRequest);
 
    // Waits for HLFB to assert (signaling a successful new torque output)
    SerialPort.SendLine("Moving... Waiting for HLFB");
    // Allow some time for HLFB to transition.
    Delay_ms(1);
    while (motor.HlfbState() != MotorDriver::HLFB_ASSERTED) {
        continue;
    }
 
    SerialPort.SendLine("Move Done");
    return true;
}
//------------------------------------------------------------------------------