From d8fab8bb252cff594a215902fe5069aac757b401 Mon Sep 17 00:00:00 2001 From: JRauer Date: Fri, 16 Jul 2021 12:25:26 +0200 Subject: [PATCH] Added jointLImits for all robots --- src/morobot.cpp | 12 ++++++++++++ src/morobot.h | 18 ++++++++++++++++++ src/morobot_2d.h | 5 +++-- src/morobot_3d.h | 3 ++- src/morobot_p.h | 3 ++- src/morobot_s_rrp.h | 3 ++- src/morobot_s_rrr.h | 3 ++- src/newRobotClass_Template.h | 3 ++- 8 files changed, 43 insertions(+), 7 deletions(-) diff --git a/src/morobot.cpp b/src/morobot.cpp index d6cee80..dd01c56 100644 --- a/src/morobot.cpp +++ b/src/morobot.cpp @@ -29,6 +29,8 @@ float getTemp(uint8_t servoId); float getVoltage(uint8_t servoId); float getCurrent(uint8_t servoId); + long getJointLimit(uint8_t servoId, bool limitNum); + uint8_t getAxisLimit(char axis, bool limitNum); uint8_t getNumSmartServos(); void moveToAngle(uint8_t servoId, long angle); @@ -244,6 +246,16 @@ float morobotClass::getCurrent(uint8_t servoId){ return smartServos.getCurrentRequest(servoId+1); } +long morobotClass::getJointLimit(uint8_t servoId, bool limitNum){ + return _robotJointLimits[servoId][limitNum]; +} + +uint8_t morobotClass::getAxisLimit(char axis, bool limitNum){ + if (axis == 'x') return _robotAxisLimits[0][limitNum]; + else if (axis == 'y') return _robotAxisLimits[1][limitNum]; + else if (axis == 'z') return _robotAxisLimits[2][limitNum]; +} + uint8_t morobotClass::getNumSmartServos(){ return _numSmartServos; } diff --git a/src/morobot.h b/src/morobot.h index af31137..3f4825d 100644 --- a/src/morobot.h +++ b/src/morobot.h @@ -29,6 +29,8 @@ float getTemp(uint8_t servoId); float getVoltage(uint8_t servoId); float getCurrent(uint8_t servoId); + long getJointLimit(uint8_t servoId, bool limitNum); + uint8_t getAxisLimit(char axis, bool limitNum); uint8_t getNumSmartServos(); void moveToAngle(uint8_t servoId, long angle); @@ -220,6 +222,20 @@ class morobotClass { * \return Current current consumption of motor in Ampere. */ float getCurrent(uint8_t servoId); + + /** + * \brief Returns the limits of a given axis + * \param [in] servoID Number of servo to get limit of + * \param [in] limitNum 0 for lower limit, 1 for upper limit + */ + long getJointLimit(uint8_t servoId, bool limitNum); + + /** + * \brief Return the limit in x/y/z orientation + * \param [in] axis Axis for which to get the limit + * \param [in] limitNum 0 for lower limit, 1 for upper limit + */ + uint8_t getAxisLimit(char axis, bool limitNum); /** * \brief Returns number of smart servos in robot @@ -417,6 +433,8 @@ class morobotClass { void printInvalidAngleError(uint8_t servoId, float angle); uint8_t _numSmartServos; //!< Number of smart servos of robot + long _robotJointLimits[3][2]; //!< Limits for all joints + uint8_t _robotAxisLimits[3][2]; //!< Limits of x, y, z axis uint8_t _speedRPM; //!< Default speed (used if movement-funtions to not provide specific speed) float _actPos[3]; //!< Robot TCP position (in base frame) float _actOri[3]; //!< Robot TCP orientation (rotation in degrees around base frame) diff --git a/src/morobot_2d.h b/src/morobot_2d.h index 0f8f992..d3cbf37 100644 --- a/src/morobot_2d.h +++ b/src/morobot_2d.h @@ -25,7 +25,7 @@ class morobot_2d:public morobotClass { * \brief Constructor of morobot_2d class * \details The value in brakets defines that the robot consists of two smartservos */ - morobot_2d() : morobotClass(2){}; + morobot_2d() : morobotClass(2){memcpy(_robotJointLimits, _jointLimits, 3*2*sizeof(long)); memcpy(_robotAxisLimits, _axisLimits, 3*2*sizeof(uint8_t));}; /** * \brief Set the position of the TCP (tool center point) with respect to the center of the flange of the last robot axis. @@ -73,7 +73,8 @@ class morobot_2d:public morobotClass { private: float _tcpOffset[3]; //!< Position of the TCP (tool center point) with respect to the center of the flange of the last robot axis - long _jointLimits[2][2] = {{-110, 135}, {-72, 24}}; //!< Limits for all joints + long _jointLimits[3][2] = {{-110, 135}, {-72, 24}, {0, 0}}; //!< Limits for all joints + uint8_t _axisLimits[3][2] = {{-20, 280}, {74.24, 74.24}, {110, 240}}; //!< Limits of x, y, z axis float x_def_offset = 40.96; //!< Default offset from side of robot to first motor float y_def_offset = 74.24; //!< Default offset from side of robot to center of eef diff --git a/src/morobot_3d.h b/src/morobot_3d.h index 7fc7609..641b1b6 100644 --- a/src/morobot_3d.h +++ b/src/morobot_3d.h @@ -26,7 +26,7 @@ class morobot_3d:public morobotClass { * \brief Constructor of morobot_3d class * \details The value in brakets defines that the robot consists of three smartservos */ - morobot_3d() : morobotClass(3){}; + morobot_3d() : morobotClass(3){memcpy(_robotJointLimits, _jointLimits, 3*2*sizeof(long)); memcpy(_robotAxisLimits, _axisLimits, 3*2*sizeof(uint8_t));}; /** * \brief Set the position of the TCP (tool center point) with respect to the center of the flange of the last robot axis. @@ -87,6 +87,7 @@ class morobot_3d:public morobotClass { private: float _tcpOffset[3]; //!< Position of the TCP (tool center point) with respect to the center of the flange of the last robot axis long _jointLimits[3][2] = {{0, 85}, {0, 85}, {0, 85}}; //!< Limits for all joints + uint8_t _axisLimits[3][2] = {{-80, 80}, {-80, 80}, {112, 235}}; //!< Limits of x, y, z axis float z_def_offset_top = 24.0; //!< Default offset from top side of robot to motor axes float z_def_offset_bottom = 10.0; //!< Default offset from bottom side of end effector triangle to rotation axis of last joint diff --git a/src/morobot_p.h b/src/morobot_p.h index ce53c21..668b315 100644 --- a/src/morobot_p.h +++ b/src/morobot_p.h @@ -26,7 +26,7 @@ class morobot_p:public morobotClass { * \brief Constructor of morobot_p class * \details The value in brakets defines that the robot consists of three smartservos */ - morobot_p() : morobotClass(3){}; + morobot_p() : morobotClass(3){memcpy(_robotJointLimits, _jointLimits, 3*2*sizeof(long)); memcpy(_robotAxisLimits, _axisLimits, 3*2*sizeof(uint8_t));}; /** * \brief Set the position of the TCP (tool center point) with respect to the center of the flange of the last robot axis. @@ -90,6 +90,7 @@ class morobot_p:public morobotClass { private: float _tcpOffset[3]; //!< Position of the TCP (tool center point) with respect to the center of the flange of the last robot axis long _jointLimits[3][2] = {{-360, 360}, {0, 115}, {-100, 28}}; //!< Limits for all joints + uint8_t _axisLimits[3][2] = {{-300, 300}, {-300, 300}, {50, 210}}; //!< Limits of x, y, z axis float d1 = 88.20; //!< Distance between base and rotational axes of motor 2 float a1 = 120; //!< Length of first link connected to motor 2 diff --git a/src/morobot_s_rrp.h b/src/morobot_s_rrp.h index 71c4786..43243ea 100644 --- a/src/morobot_s_rrp.h +++ b/src/morobot_s_rrp.h @@ -27,7 +27,7 @@ class morobot_s_rrp:public morobotClass { * \brief Constructor of morobot_s_rrp class * \details The value in brakets defines that the robot consists of three smartservos */ - morobot_s_rrp() : morobotClass(3){}; + morobot_s_rrp() : morobotClass(3){memcpy(_robotJointLimits, _jointLimits, 3*2*sizeof(long)); memcpy(_robotAxisLimits, _axisLimits, 3*2*sizeof(uint8_t));}; /** * \brief Set the position of the TCP (tool center point) with respect to the center of the flange of the last robot axis. @@ -95,6 +95,7 @@ class morobot_s_rrp:public morobotClass { private: float _tcpOffset[3]; //!< Position of the TCP (tool center point) with respect to the center of the flange of the last robot axis long _jointLimits[3][2] = {{-100, 100}, {-100, 100}, {0, 780}}; //!< Limits for all joints + uint8_t _axisLimits[3][2] = {{-35, 210}, {-165, 165}, {-40, 0}}; //!< Limits of x, y, z axis float a = 47.0; //!< Length from mounting to first axis float b = 92.9; //!< Length from first axis to second axis diff --git a/src/morobot_s_rrr.h b/src/morobot_s_rrr.h index 4d77e66..b5e93fc 100644 --- a/src/morobot_s_rrr.h +++ b/src/morobot_s_rrr.h @@ -26,7 +26,7 @@ class morobot_s_rrr:public morobotClass { * \brief Constructor of morobot_s_rrr class * \details The value in brakets defines that the robot consists of three smartservos */ - morobot_s_rrr() : morobotClass(3){}; + morobot_s_rrr() : morobotClass(3){memcpy(_robotJointLimits, _jointLimits, 3*2*sizeof(long)); memcpy(_robotAxisLimits, _axisLimits, 3*2*sizeof(uint8_t));}; /** * \brief Set the position of the TCP (tool center point) with respect to the center of the flange of the last robot axis. @@ -87,6 +87,7 @@ class morobot_s_rrr:public morobotClass { private: float _tcpOffset[3]; //!< Position of the TCP (tool center point) with respect to the center of the flange of the last robot axis long _jointLimits[3][2] = {{-100, 100}, {-100, 100}, {-180, 180}}; //!< Limits for all joints + uint8_t _axisLimits[3][2] = {{-100, 100}, {-100, 100}, {-50, 50}}; //!< Limits of x, y, z axis float a = 47.0; //!< Length from mounting to first axis float b = 92.9; //!< Length from first axis to second axis diff --git a/src/newRobotClass_Template.h b/src/newRobotClass_Template.h index 3986733..5caa3a7 100644 --- a/src/newRobotClass_Template.h +++ b/src/newRobotClass_Template.h @@ -25,7 +25,7 @@ class newRobotClass_Template:public morobotClass { * \brief Constructor of newRobotClass_Template class * \details The value in brakets defines the number of smart servo motors */ - newRobotClass_Template() : morobotClass(3){}; // TODO: PUT THE NUMBER OF SERVOS HERE + newRobotClass_Template() : morobotClass(3){memcpy(_robotJointLimits, _jointLimits, 3*2*sizeof(long)); memcpy(_robotAxisLimits, _axisLimits, 3*2*sizeof(uint8_t));}; // TODO: PUT THE NUMBER OF SERVOS HERE /** * \brief Set the position of the TCP (tool center point) with respect to the center of the flange of the last robot axis. @@ -76,6 +76,7 @@ class newRobotClass_Template:public morobotClass { private: float _tcpOffset[3]; //!< Position of the TCP (tool center point) with respect to the center of the flange of the last robot axis long _jointLimits[3][2] = {{-100, 100}, {-100, 100}, {0, 780}}; //!< Limits for all joints + uint8_t _axisLimits[3][2] = {{-100, 100}, {-100, 100}, {-50, 50}}; //!< Limits of x, y, z axis // TODO: CHANGE THE NUMBER OF SERVOS AND THE LIMITS HERE //TODO: PUT VARIABLES FOR INVERSE KINEMATICS HERE