osaRobot1394.h

Go to the documentation of this file.

/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*-    */
/* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */

/*
  Author(s):  Zihan Chen, Peter Kazanzides, Anton Deguet
  Created on: 2011-06-10

  (C) Copyright 2011-2015 Johns Hopkins University (JHU), All Rights Reserved.

--- begin cisst license - do not edit ---

This software is provided "as is" under an open source license, with
no warranty.  The complete license can be found in license.txt and
http://www.cisst.org/cisst/license.txt.

--- end cisst license ---
*/

#ifndef _osaRobot1394_h
#define _osaRobot1394_h

#include <vector>
#include <map>
#include <stdint.h>

#ifndef SAW_ROBOT_IO_1394_WO_CISST
#include <cisstVector/vctDynamicVectorTypes.h>
#include <cisstParameterTypes/prmJointType.h>
#else
#include <boost/shared_ptr.hpp>
#include <Eigen/Dense>
#include "EigenWrapper.h"
#include "MinimalPrm.h"
#endif

#include <sawRobotIO1394/osaConfiguration1394.h>

class AmpIO;

namespace sawRobotIO1394 {

class osaRobot1394
{
    public:
        static const size_t WATCHDOG_MS_TO_COUNT = 192;

        osaRobot1394(const osaRobot1394Configuration & config,
                     const size_t maxConsecutiveCurrentSafetyViolations = 100,
                     const size_t maxConsecutivePotsToEncodersViolations = 200);

        void Configure(const osaRobot1394Configuration & config);

        void SetBoards(const std::vector<osaActuatorMapping> & actuatorBoards,
                       const std::vector<osaBrakeMapping> & brakeBoards);

        void PollValidity(void);
        void PollState(void);
        void ConvertState(void);
        void CheckState(void);
        //
        void EnablePower(void);
        void EnableBoardsPower(void);
        void DisablePower(void);
        void DisableBoardPower(void);
        void WriteSafetyRelay(const bool & enabled);
        void SetWatchdogPeriod(const double & periodInSeconds);

        void SetActuatorPower(const bool & enabled);
        void SetActuatorPower(const vctBoolVec & enabled);
        void SetBrakePower(const bool & enabled);
        void SetBrakePower(const vctBoolVec & enabled);

        void SetEncoderPosition(const vctDoubleVec & pos);
        void SetEncoderPositionBits(const vctIntVec & bits);
        void SetSingleEncoderPosition(const int index, const double pos = 0);
        void SetSingleEncoderPositionBits(const int index, const int bits = 0);

        void SetJointEffort(const vctDoubleVec & efforts);
        void SetActuatorEffort(const vctDoubleVec & efforts);
        void SetActuatorCurrent(const vctDoubleVec & currents);
        void SetActuatorCurrentBits(const vctIntVec & bits);

        void UsePotsForSafetyCheck(const bool & usePotsForSafetyCheck);
        void SetPotsToEncodersTolerance(const vctDoubleVec & tolerances);

        void SetBrakeCurrent(const vctDoubleVec & currents);
        void SetBrakeCurrentBits(const vctIntVec & bits);
        void BrakeRelease(void);
        void BrakeEngage(void);
        bool Valid(void) const;
        bool PowerStatus(void) const;
        bool SafetyRelay(void) const;
        bool WatchdogStatus(void) const;
        const vctBoolVec & ActuatorPowerStatus(void) const;
        const vctBoolVec & BrakePowerStatus(void) const;
        const vctDoubleVec & ActuatorCurrentFeedback(void) const;
        const vctDoubleVec & BrakeCurrentFeedback(void) const;
        const vctDoubleVec & PotPosition(void) const;
        const vctDoubleVec & ActuatorTimeStamp(void) const;
        const vctDoubleVec & BrakeTimeStamp(void) const;
        const vctDoubleVec & EncoderPosition(void) const;
        const vctDoubleVec & EncoderVelocity(void) const;
        const vctDoubleVec & EncoderVelocitySoftware(void) const;
        osaRobot1394Configuration GetConfiguration(void) const;
        std::string Name(void) const;
        size_t NumberOfJoints(void) const;
        size_t NumberOfActuators(void) const;
        size_t SerialNumber(void) const;
        size_t NumberOfBrakes(void) const;
        void GetJointTypes(prmJointTypeVec & jointTypes) const;
        void GetJointEffortCommandLimits(vctDoubleVec & limits) const;
        void GetActuatorEffortCommandLimits(vctDoubleVec & limits) const;
        void GetActuatorCurrentCommandLimits(vctDoubleVec & limits) const;
        void CalibrateEncoderOffsetsFromPots(void);
        void EncoderPositionToBits(const vctDoubleVec & pos, vctIntVec & bits) const;
        void EncoderBitsToPosition(const vctIntVec & bits, vctDoubleVec & pos) const;
        void EncoderBitsToDPosition(const vctIntVec & bits, vctDoubleVec & dpos) const;
        void EncoderBitsToDTime(const vctIntVec & bits, vctDoubleVec & dt) const;
        void EncoderBitsToVelocity(const vctIntVec & bits, vctDoubleVec & vel) const;

        void ActuatorEffortToCurrent(const vctDoubleVec & efforts, vctDoubleVec & currents) const;
        void ActuatorCurrentToBits(const vctDoubleVec & currents, vctIntVec & bits) const;
        void ActuatorBitsToCurrent(const vctIntVec & bits, vctDoubleVec & currents) const;
        void ActuatorCurrentToEffort(const vctDoubleVec & currents, vctDoubleVec & efforts) const;

        void BrakeCurrentToBits(const vctDoubleVec & currents, vctIntVec & bits) const;
        void BrakeBitsToCurrent(const vctIntVec & bits, vctDoubleVec & currents) const;

        void PotBitsToVoltage(const vctIntVec & bits, vctDoubleVec & voltages) const;
        void PotVoltageToPosition(const vctDoubleVec & voltages, vctDoubleVec & pos) const;
    protected:

        void ClipActuatorEffort(vctDoubleVec & efforts);
        void ClipActuatorCurrent(vctDoubleVec & currents);
        void ClipBrakeCurrent(vctDoubleVec & currents);

        std::vector<osaActuatorMapping> mActuatorInfo;
        std::vector<osaBrakeMapping> mBrakeInfo;
        std::map<int, AmpIO*> mUniqueBoards;
        typedef std::map<int, AmpIO*>::iterator unique_board_iterator;
        typedef std::map<int, AmpIO*>::const_iterator unique_board_const_iterator;

        osaRobot1394Configuration mConfiguration;
        std::string mName;
        size_t mNumberOfActuators;
        size_t mNumberOfJoints;
        size_t mNumberOfBrakes;
        size_t mSerialNumber;

        // state of brakes
        bool mBrakeReleasing;
        vctDoubleVec mBrakeReleasingTimer;

        vctIntVec
            mCountsPerTurn;

        vctDoubleVec
            mEffortToCurrentScales,
            mActuatorCurrentToBitsScales,
            mBrakeCurrentToBitsScales,
            mActuatorCurrentToBitsOffsets,
            mBrakeCurrentToBitsOffsets,
            mActuatorBitsToCurrentScales,
            mBrakeBitsToCurrentScales,
            mActuatorBitsToCurrentOffsets,
            mBrakeBitsToCurrentOffsets,
            mBitsToPositionScales,
            mBitsToPositionOffsets,
            mBitsToDPositionScales,
            mBitsToDPositionOffsets,
            mBitsToDTimeScales,
            mBitsToDTimeOffsets,
            mBitsToVecocityScales,
            mBitsToVelocityOffsets,
            mBitsToVoltageScales,
            mBitsToVoltageOffsets,
            mVoltageToPositionScales,
            mVoltageToPositionOffsets;

        vctDoubleVec
            mJointEffortCommandLimits,
            mActuatorEffortCommandLimits,
            mActuatorCurrentCommandLimits,
            mBrakeCurrentCommandLimits,
            mActuatorCurrentFeedbackLimits, // limit used to trigger error
            mBrakeCurrentFeedbackLimits,    // limit used to trigger error
            mPotsToEncodersTolerance;       // maximum error between encoders and pots

        prmJointTypeVec mJointType;
        osaPot1394Location mPotType;
        bool mUsePotsForSafetyCheck;

        bool
            mValid,
            mPowerStatus,
            mPreviousPowerStatus,
            mWatchdogStatus,
            mPreviousWatchdogStatus,
            mIsAllBoardsFirmWareFour;

        unsigned short mSafetyRelay;

        vctBoolVec
            mActuatorPowerStatus,
            mBrakePowerStatus,
            mActuatorPowerEnabled,
            mPotsToEncodersErrorFlag,
            mBrakePowerEnabled,
            mPreviousEncoderOverflow,
            mEncoderOverflow,
            mDigitalInputs,
            mEncoderChannelsA;

        vctIntVec
            mPotBits,
            mEncoderPositionBits,
            mEncoderPositionBitsPrev,
            mEncoderVelocityBits,     // latched velocity
            mEncoderVelocityBitsNow,  // current counting velocity bits
            mEncoderDPositionBits,
            mEncoderDTimeBits;

        vctIntVec
            mActuatorCurrentBitsCommand,
            mBrakeCurrentBitsCommand,
            mActuatorCurrentBitsFeedback,
            mBrakeCurrentBitsFeedback;

        vctDoubleVec
            mActuatorTimestamp,
            mActuatorTimestampChange, // cumulated time since last encoder changed
            mVelocitySlopeToZero, // slope used to reduced velocity to zero when no encoder count change
            mBrakeTimestamp,
            mPotVoltage,
            mPotPosition,
            mEncoderPosition,
            mEncoderPositionPrev,
            mEncoderVelocity,
            mEncoderVelocityDxDt,
            mEncoderDPosition,
            mEncoderDTime,
            mEncoderVelocitySoftware,
            mJointPosition,
            mJointVelocity,
            mJointTorque,
            mActuatorCurrentCommand,
            mBrakeCurrentCommand,
            mActuatorEffortCommand,
            mActuatorCurrentFeedback,
            mPotsToEncodersError,
            mBrakeCurrentFeedback,
            mActuatorEffortFeedback,
            mActuatorTemperature,
            mBrakeTemperature,
            mBrakeReleaseCurrent,
            mBrakeReleaseTime,
            mBrakeReleasedCurrent,
            mBrakeEngagedCurrent;

        size_t
            mCurrentSafetyViolationsCounter,
            mCurrentSafetyViolationsMaximum;

        size_t
            mPotsToEncodersViolationsCounter,
            mPotsToEncodersViolationsMaximum;
    };

} // namespace sawRobotIO1394

#endif // _osaRobot1394_h