MotorDriver.h

Go to the documentation of this file.

/*
 * Copyright (c) 2020 Teknic, Inc.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */
 
#ifndef __MOTORDRIVER_H__
#define __MOTORDRIVER_H__
 
#include <stdint.h>
#include "Connector.h"
#include "DigitalIn.h"
#include "PeripheralRoute.h"
#include "ShiftRegister.h"
#include "StatusManager.h"
#include "StepGenerator.h"
#include "SysUtils.h"
#include "SysManager.h"
 
#define HLFB_CARRIER_LOSS_ERROR_LIMIT (0)
#define HLFB_CARRIER_LOSS_STATE_CHANGE_MS_45_HZ (25)
#define HLFB_CARRIER_LOSS_STATE_CHANGE_MS_482_HZ (4)
 
namespace ClearCore {
 
#define CPM_HLFB_CAP_HISTORY 2
 
#define CPM_ENABLE_DELAY 1250
 
#define DEFAULT_TRIGGER_PULSE_WIDTH_MS  25
 
extern SysManager SysMgr;
 
class MotorDriver : public DigitalIn, public StepGenerator {
    friend class MotorManager;
    friend class StatusManager;
    friend class SysManager;
 
public:
    union PolarityInversionsSD {
        int16_t reg;
 
        struct {
            uint32_t enableInverted : 1;
            uint32_t directionInverted : 1;
            uint32_t hlfbInverted : 1;
        } bit;
 
        PolarityInversionsSD() {
            reg = 0;
        }
 
        PolarityInversionsSD(int16_t val) {
            reg = val;
        }
    };
 
    static const int16_t HLFB_DUTY_UNKNOWN = -9999;
 
    typedef enum {
        HLFB_DEASSERTED,
        HLFB_ASSERTED,
        HLFB_HAS_MEASUREMENT,
        HLFB_UNKNOWN
    } HlfbStates;
 
    typedef enum {
        HLFB_MODE_STATIC,
        HLFB_MODE_HAS_PWM,
        HLFB_MODE_HAS_BIPOLAR_PWM
    } HlfbModes;
 
    typedef enum {
        HLFB_CARRIER_45_HZ,
        HLFB_CARRIER_482_HZ
    } HlfbCarrierFrequency;
 
    typedef enum {
        MOTOR_DISABLED = 0,
        MOTOR_ENABLING,
        MOTOR_FAULTED,
        MOTOR_READY,
        MOTOR_MOVING
    } MotorReadyStates;
 
    union StatusRegMotor {
        uint32_t reg;
 
        struct {
            uint32_t AtTargetPosition : 1;
            uint32_t StepsActive : 1;
            uint32_t AtTargetVelocity : 1;
            uint32_t MoveDirection : 1;
            uint32_t MotorInFault : 1;
            uint32_t Enabled : 1;
            uint32_t PositionalMove : 1;
            uint32_t HlfbState : 2;
            uint32_t AlertsPresent : 1;
            MotorReadyStates ReadyState : 3;
            uint32_t Triggering : 1;
            uint32_t InPositiveLimit : 1;
            uint32_t InNegativeLimit : 1;
            uint32_t InEStopSensor : 1;
        } bit;
 
        StatusRegMotor() {
            reg = 0;
        }
 
        StatusRegMotor(uint32_t val) {
            reg = val;
        }
    };
 
    union AlertRegMotor {
        uint32_t reg;
 
        struct {
            uint16_t MotionCanceledInAlert : 1;
            uint16_t MotionCanceledPositiveLimit : 1;
            uint16_t MotionCanceledNegativeLimit : 1;
            uint16_t MotionCanceledSensorEStop : 1;
            uint16_t MotionCanceledMotorDisabled : 1;
            uint16_t MotorFaulted : 1;
        } bit;
 
        AlertRegMotor() {
            reg = 0;
        }
 
        AlertRegMotor(uint32_t val) {
            reg = val;
        }
    };
 
    bool ValidateMove(bool negDirection);
 
    virtual bool Move(int32_t dist,
                      MoveTarget moveTarget = MOVE_TARGET_REL_END_POSN) override;
 
    virtual bool MoveVelocity(int32_t velocity) override;
 
    void HlfbFilterLength(uint16_t samples) {
        DigitalIn::FilterLength(samples);
    }
 
    Connector::ConnectorTypes Type() override {
        return Connector::CPM_TYPE;
    }
 
    bool IsWritable() override {
        return true;
    }
 
    bool MotorInAState();
 
    bool MotorInAState(bool value);
 
    bool MotorInBState();
 
    bool MotorInBState(bool value);
 
    bool EnableRequest() {
        return m_enableRequestedState;
    }
 
    void EnableRequest(bool value);
 
    bool MotorInADuty(uint8_t duty);
 
    bool MotorInBDuty(uint8_t duty);
 
    void EnableTriggerPulse(uint16_t pulseCount = 1,
                            uint32_t time_ms = DEFAULT_TRIGGER_PULSE_WIDTH_MS,
                            bool blockUntilDone = false);
 
    volatile const bool &EnableTriggerPulseActive() {
        return m_enableTriggerActive;
    }
 
    volatile const HlfbStates &HlfbState() {
        return m_hlfbState;
    }
 
    volatile const float &HlfbPercent() {
        return m_hlfbDuty;
    }
 
    void HlfbMode(HlfbModes newMode) {
        if (m_hlfbMode == newMode) {
            return;
        }
        m_hlfbMode = newMode;
        m_hlfbCarrierLost = true;
        m_hlfbDuty = HLFB_DUTY_UNKNOWN;
    }
 
    HlfbModes HlfbMode() {
        return m_hlfbMode;
    }
 
    bool HlfbHasRisen() {
        return DigitalIn::InputRisen();
    }
 
    bool HlfbHasFallen() {
        return DigitalIn::InputFallen();
    }
 
    bool HlfbCarrier(HlfbCarrierFrequency freq) {
        switch (freq) {
            case HLFB_CARRIER_45_HZ:
                m_hlfbCarrierLossStateChange_ms =
                    HLFB_CARRIER_LOSS_STATE_CHANGE_MS_45_HZ;
                break;
            case HLFB_CARRIER_482_HZ:
                m_hlfbCarrierLossStateChange_ms =
                    HLFB_CARRIER_LOSS_STATE_CHANGE_MS_482_HZ;
                break;
            default:
                return false;
        }
        m_hlfbCarrierFrequency = freq;
        return true;
    }
 
    HlfbCarrierFrequency HlfbCarrier() {
        return m_hlfbCarrierFrequency;
    }
 
    bool IsInHwFault() override {
        return (volatile bool &)m_inFault;
    }
    volatile const StatusRegMotor &StatusReg() {
        return m_statusRegMotor;
    }
 
    StatusRegMotor StatusRegRisen();
 
    StatusRegMotor StatusRegFallen();
 
    volatile const AlertRegMotor &AlertReg() {
        return m_alertRegMotor;
    }
 
    void ClearAlerts(uint32_t mask = UINT32_MAX) {
        atomic_and_fetch(&m_alertRegMotor.reg, ~mask);
    }
 
    bool PolarityInvertSDEnable(bool invert);
    bool PolarityInvertSDDirection(bool invert);
    bool PolarityInvertSDHlfb(bool invert);
 
    bool BrakeOutput(ClearCorePins pin);
 
    ClearCorePins BrakeOutput() {
        return m_brakeOutputPin;
    }
 
    bool LimitSwitchPos(ClearCorePins pin);
 
    ClearCorePins LimitSwitchPos() {
        return m_limitSwitchPos;
    }
 
    bool LimitSwitchNeg(ClearCorePins pin);
 
    ClearCorePins LimitSwitchNeg() {
        return m_limitSwitchNeg;
    }
 
    virtual ConnectorModes Mode() override {
        return Connector::Mode();
    }
 
    bool EnableConnector(ClearCorePins pin);
 
    ClearCorePins EnableConnector() {
        return m_enableConnector;
    }
 
    bool InputAConnector(ClearCorePins pin);
 
    ClearCorePins InputAConnector() {
        return m_inputAConnector;
    }
 
    bool InputBConnector(ClearCorePins pin);
 
    ClearCorePins InputBConnector() {
        return m_inputBConnector;
    }
 
    bool EStopConnector(ClearCorePins pin);
 
    ClearCorePins EStopConnector() {
        return m_eStopConnector;
    }
 
    bool HlfbInputStatus() {
        return m_hlfbState == HLFB_ASSERTED ||
               m_hlfbState == HLFB_HAS_MEASUREMENT;
    }
 
    void EnableActiveLevel(bool activeLevel) {
        m_polarityInversions.bit.enableInverted = activeLevel;
    }
 
    bool EnableActiveLevel() {
        return m_polarityInversions.bit.enableInverted;
    }
 
    void HlfbActiveLevel(bool activeLevel) {
        m_polarityInversions.bit.hlfbInverted = !activeLevel;
    }
 
    bool HlfbActiveLevel() {
        return !m_polarityInversions.bit.hlfbInverted;
    }
 
#ifndef HIDE_FROM_DOXYGEN
 
    virtual void OutputDirection() override {
        if (m_mode == Connector::CPM_MODE_STEP_AND_DIR &&
                m_polarityInversions.bit.directionInverted) {
            DATA_OUTPUT_STATE(m_aInfo->gpioPort, m_aDataMask, Direction());
        }
        else {
            DATA_OUTPUT_STATE(m_aInfo->gpioPort, m_aDataMask, !Direction());
        }
    }
 
    void ClearFaults(uint32_t disableTime_ms, uint32_t waitForHlfbTime_ms = 0) {
        EnableTriggerPulse(1, disableTime_ms);
        m_clearFaultHlfbTimer = waitForHlfbTime_ms;
        m_clearFaultState = CLEAR_FAULT_PULSE_ENABLE;
    }
 
    bool ClearFaultsActive() {
        return m_clearFaultState != CLEAR_FAULT_IDLE;
    }
 
    static bool IsValidInputPin(ClearCorePins pin);
 
    static bool IsValidOutputPin(ClearCorePins pin);
 
    bool MotorInACount(uint16_t count);
 
    bool MotorInBCount(uint16_t count);
 
    MotorDriver() {};
#endif
protected:
    // Enable bit associated with this CPM
    ShiftRegister::Masks m_enableMask;
 
    // Mapping to all the pin information
    const PeripheralRoute *m_aInfo;
    const PeripheralRoute *m_bInfo;
    const PeripheralRoute *m_hlfbInfo;
 
    // Keep some commonly-used bits from the Info structures
    uint32_t m_aDataMask;
    volatile uint32_t *m_aTccBuffer;
    uint32_t m_bDataMask;
    volatile uint32_t *m_bTccBuffer;
    uint32_t m_aTccSyncMask;
    volatile uint32_t *m_aTccSyncReg;
    uint32_t m_bTccSyncMask;
    volatile uint32_t *m_bTccSyncReg;
 
    // Enable, InA, InB connector pairing
    ClearCorePins m_enableConnector;
    ClearCorePins m_inputAConnector;
    ClearCorePins m_inputBConnector;
 
    // - - - - - - - - - - - - - - - - - - - - - - - - - - -
    // HLFB State
    // - - - - - - - - - - - - - - - - - - - - - - - - - - -
    // Timer/Counter index associated with HLFB input
    uint8_t m_hlfbTcNum;
    // Assigned channel in EVSYS
    uint8_t m_hlfbEvt;
    // HLFB measurement mode
    HlfbModes m_hlfbMode;
    // HLFB width, period raw measurements
    uint16_t m_hlfbWidth[CPM_HLFB_CAP_HISTORY];
    uint16_t m_hlfbPeriod[CPM_HLFB_CAP_HISTORY];
    // HLFB measurement count, used to show lack of PWM
    uint16_t m_hlfbNoPwmSampleCount;
    HlfbCarrierFrequency m_hlfbCarrierFrequency;
    uint32_t m_hlfbCarrierLossStateChange_ms;
    // The last board time (in milliseconds) when PWM carrier was detected
    uint32_t m_hlfbLastCarrierDetectTime;
    // HLFB last duty cycle
    float m_hlfbDuty;
    // HLFB state return
    HlfbStates m_hlfbState;
    bool m_lastHlfbInputValue;
    bool m_hlfbPwmReadingPending;
    uint16_t m_hlfbStateChangeCounter;
 
    // Inversion mask of actual enable, direction, and HLFB state
    PolarityInversionsSD m_polarityInversions;
 
    bool m_enableRequestedState;
    bool m_enableTriggerActive;
    uint32_t m_enableTriggerPulseStartMs;
    uint32_t m_enableTriggerPulseCount;
    uint32_t m_enableTriggerPulseLenMs;
 
    uint16_t m_aDutyCnt;
    uint16_t m_bDutyCnt;
 
    bool m_inFault;
 
    StatusRegMotor m_statusRegMotor;
    StatusRegMotor m_statusRegMotorRisen;
    StatusRegMotor m_statusRegMotorFallen;
    StatusRegMotor m_statusRegMotorLast;
 
    AlertRegMotor m_alertRegMotor;
 
    void Initialize(ClearCorePins clearCorePin) override;
 
    void ToggleEnable();
 
private:
 
    enum ClearFaultState {
        CLEAR_FAULT_IDLE,
        CLEAR_FAULT_PULSE_ENABLE,
        CLEAR_FAULT_WAIT_FOR_HLFB
    };
 
    bool m_initialized;
 
    // Internal fields used for the IsReady field of the motor status reg
    bool m_isEnabling;
    bool m_isEnabled;
    bool m_hlfbCarrierLost;
    int32_t m_enableCounter;
 
    // Brake Output Feature
    ClearCorePins m_brakeOutputPin;
 
    // Limit Switch Feature
    ClearCorePins m_limitSwitchNeg;
    ClearCorePins m_limitSwitchPos;
 
    // Hardware E-Stop Sensor Feature
    ClearCorePins m_eStopConnector;
    bool m_motionCancellingEStop;
 
    bool m_shiftRegEnableReq;
    ClearFaultState m_clearFaultState;
    uint32_t m_clearFaultHlfbTimer;
 
    MotorDriver(enum ShiftRegister::Masks enableMask,
                const PeripheralRoute *aInfo,
                const PeripheralRoute *bInfo,
                const PeripheralRoute *hlfbInfo,
                uint16_t hlfbTc,
                uint16_t hlfbEvt);
 
    void UpdateADuty();
    void UpdateBDuty();
 
    void RefreshSlow();
 
    // Poll electrical connector state and update the internal state.
    void Refresh() override;
 
    void FaultState(bool isFaulted);
 
    bool Mode(Connector::ConnectorModes newMode) override;
 
    bool SetConnector(ClearCorePins pin, ClearCorePins &memberPin,
                      bool input = true);
 
    bool CheckEStopSensor();
 
}; // MotorDriver
 
} // ClearCore namespace
 
#endif // __MOTORDRIVER_H__