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__