MotorizedJoint.h

#pragma once

#include "PidControl.h"

namespace AnimatSim
{
        namespace Environment
        {

                enum eJointMotorType
                {
            VelocityControl,
            PositionControl,
            PositionVelocityControl
                };

                class ANIMAT_PORT MotorizedJoint : public Joint
                {
                protected:
                        IMotorizedJoint *m_lpPhysicsMotorJoint;

                        float m_fltSetPosition;

                        float m_fltDesiredPosition;

                        float m_fltReportSetPosition;

                        float m_fltPrevSetPosition;

                        bool m_bReachedSetPos;

                        float m_fltSetVelocity;

                        float m_fltPrevSetVelocity;

                        float m_fltDesiredVelocity;

                        float m_fltReportSetVelocity;

                        float m_fltMaxVelocity;

                        float m_fltPrevVelocity;

                        bool m_bEnableMotor;

                        bool m_bEnableMotorInit;

                        float m_fltMaxForce;

                        float m_fltMaxForceNotScaled;

                        float m_ftlServoGain;

                        eJointMotorType m_eMotorType;

            int m_iAssistCountdown;

            CStdFPoint m_vMotorForceToA;

            float m_fltMotorForceAMagnitude;

            CStdFPoint m_vMotorAssistForceToA;

            CStdFPoint m_vMotorAssistForceToAReport;

            CStdFPoint m_vMotorForceToB;

            float m_fltMotorForceBMagnitude;

            CStdFPoint m_vMotorAssistForceToB;

            CStdFPoint m_vMotorAssistForceToBReport;

            CStdFPoint m_vMotorTorqueToA;

            float m_fltMotorTorqueAMagnitude;

            CStdFPoint m_vMotorAssistTorqueToA;

            CStdFPoint m_vMotorAssistTorqueToAReport;

            CStdFPoint m_vMotorTorqueToB;

            float m_fltMotorTorqueBMagnitude;

            CStdFPoint m_vMotorAssistTorqueToB;

            CStdFPoint m_vMotorAssistTorqueToBReport;

            float m_fltMotorAssistMagnitude;

            float m_fltMotorAssistMagnitudeReport;

            PidControl *m_lpAssistPid;

                        //This is the temperature for the motor. This is not really that useful in the simulation at the moment, but
                        //it is data that is available for robotic motors, so I am adding a hook here for setting and displaying it.
                        float m_fltTemperature;

                        //This is the voltage for the motor. This is not really that useful in the simulation at the moment, but
                        //it is data that is available for robotic motors, so I am adding a hook here for setting and displaying it.
                        float m_fltVoltage;

                        RobotPartInterface *m_lpRobotMotorControl;
                        
                        float m_fltSetPositionDeltaCheck;

            virtual void AddRobotPartInterface(RobotPartInterface *lpPart);
            virtual void RemoveRobotPartInterface(RobotPartInterface *lpPart);

            virtual void ClearAssistForces();
            virtual void ApplyMotorAssist();
            virtual void EnableFeedback();

                public:
                        MotorizedJoint(void);
                        virtual ~MotorizedJoint(void);

                        static MotorizedJoint *CastToDerived(AnimatBase *lpBase) {return static_cast<MotorizedJoint*>(lpBase);}

                        virtual IMotorizedJoint *PhysicsMotorJoint();
                        virtual void PhysicsMotorJoint(IMotorizedJoint *lpJoint);

                        virtual bool EnableMotor();
                        virtual void EnableMotor(bool bVal);

                        virtual void MotorType(eJointMotorType eServo);
                        virtual eJointMotorType MotorType();

                        virtual void ServoGain(float fltVal);
                        virtual float ServoGain();

                        virtual void MaxForce(float fltVal, bool bUseScaling = true);
                        virtual float MaxForce();
                        virtual float MaxForceNotScaled();

                        virtual float MaxVelocity();
                        virtual void MaxVelocity(float fltVal, bool bUseScaling = true);

                        virtual float DesiredPosition();
                        virtual void DesiredPosition(float fltPosition);

                        virtual float SetPosition();
                        virtual void SetPosition(float fltVal);

                        virtual float PrevSetPosition();
                        virtual void PrevSetPosition(float fltVal);

                        virtual bool ReachedSetPosition();
                        virtual void ReachedSetPosition(bool bVal);

                        virtual float DesiredVelocity();
                        virtual void DesiredVelocity(float fltVelocity);

                        virtual float SetVelocity();
                        virtual void SetVelocity(float fltVal);

                        virtual float PrevSetVelocity();
                        virtual void PrevSetVelocity(float fltVal);

                        virtual float PrevVelocity();
                        virtual void PrevVelocity(float fltVal);

            virtual int AssistCountdown();
            virtual void AssistCountdown(int iVal);

            virtual CStdFPoint MotorForceToA();
            virtual void MotorForceToA(CStdFPoint &vVal);

            virtual float MotorForceToAMagnitude();
            virtual void MotorForceToAMagnitude(float fltVal);

            virtual CStdFPoint MotorAssistForceToA();
            virtual void MotorAssistForceToA(CStdFPoint &vVal);

            virtual CStdFPoint MotorAssistForceToAReport();
            virtual void MotorAssistForceToAReport(CStdFPoint &vVal);

            virtual CStdFPoint MotorForceToB();
            virtual void MotorForceToB(CStdFPoint &vVal);

            virtual float MotorForceToBMagnitude();
            virtual void MotorForceToBMagnitude(float fltVal);

            virtual CStdFPoint MotorAssistForceToB();
            virtual void MotorAssistForceToB(CStdFPoint &vVal);

            virtual CStdFPoint MotorAssistForceToBReport();
            virtual void MotorAssistForceToBReport(CStdFPoint &vVal);

            virtual CStdFPoint MotorTorqueToA();
            virtual void MotorTorqueToA(CStdFPoint &vVal);

            virtual float MotorTorqueToAMagnitude();
            virtual void MotorTorqueToAMagnitude(float fltVal);

            virtual CStdFPoint MotorAssistTorqueToA();
            virtual void MotorAssistTorqueToA(CStdFPoint &vVal);

            virtual CStdFPoint MotorAssistTorqueToAReport();
            virtual void MotorAssistTorqueToAReport(CStdFPoint &vVal);

            virtual CStdFPoint MotorTorqueToB();
            virtual void MotorTorqueToB(CStdFPoint &vVal);

            virtual float MotorTorqueToBMagnitude();
            virtual void MotorTorqueToBMagnitude(float fltVal);

            virtual CStdFPoint MotorAssistTorqueToB();
            virtual void MotorAssistTorqueToB(CStdFPoint &vVal);

            virtual CStdFPoint MotorAssistTorqueToBReport();
            virtual void MotorAssistTorqueToBReport(CStdFPoint &vVal);

            virtual CStdPID *AssistPid();

                        virtual float Temperature();
                        virtual void Temperature(float fltVal);

                        virtual float Voltage();
                        virtual void Voltage(float fltVal);

                        virtual void MotorInput(float fltInput);

                        virtual void RobotMotorControl(RobotPartInterface *lpPart);
                        virtual RobotPartInterface *RobotMotorControl();

                        virtual void SetVelocityToDesired();
                        virtual void EnableLock(bool bOn, float fltPosition, float fltMaxLockForce);

            virtual void Initialize();
                        virtual void ResetSimulation();

                        virtual float *GetDataPointer(const std::string &strDataType);
                        virtual bool SetData(const std::string &strDataType, const std::string &strValue, bool bThrowError = true);
                        virtual void QueryProperties(CStdPtrArray<TypeProperty> &aryProperties);

                        virtual void Load(CStdXml &oXml);
                };

        }
}