From 9dd6ab94ad2ed747fe6afbb33adc88d81e88be73 Mon Sep 17 00:00:00 2001 From: 2L Date: Wed, 17 Feb 2021 22:15:01 +0100 Subject: [PATCH] Different shanks length support; Improved position validation; --- .../robotArm_v0.31/RampsStepper.cpp | 2 +- arduino_firmware/robotArm_v0.31/config.h | 18 +++++++---- .../robotArm_v0.31/interpolation.cpp | 30 +++++++++++-------- .../robotArm_v0.31/robotArm_v0.31.ino | 4 +-- .../robotArm_v0.31/robotGeometry.cpp | 21 ++++++++----- .../robotArm_v0.31/robotGeometry.h | 5 ++-- 6 files changed, 49 insertions(+), 31 deletions(-) diff --git a/arduino_firmware/robotArm_v0.31/RampsStepper.cpp b/arduino_firmware/robotArm_v0.31/RampsStepper.cpp index 019c85e..a37dc1c 100644 --- a/arduino_firmware/robotArm_v0.31/RampsStepper.cpp +++ b/arduino_firmware/robotArm_v0.31/RampsStepper.cpp @@ -1,4 +1,4 @@ -#include "rampsStepper.h" +#include "RampsStepper.h" #include "config.h" #include diff --git a/arduino_firmware/robotArm_v0.31/config.h b/arduino_firmware/robotArm_v0.31/config.h index 0bd2e07..ab1ab1e 100644 --- a/arduino_firmware/robotArm_v0.31/config.h +++ b/arduino_firmware/robotArm_v0.31/config.h @@ -5,14 +5,15 @@ #define BAUD 115200 //ROBOT ARM LENGTH -#define SHANK_LENGTH 140.0 +#define LOW_SHANK_LENGTH 140.0 +#define HIGH_SHANK_LENGTH 140.0 #define END_EFFECTOR_OFFSET 55.0 // LENGTH FROM UPPER SHANK BEARING TO MIDPOINT OF END EFFECTOR IN MM //INITIAL INTERPOLATION SETTINGS // INITIAL_XYZ FORMS VERTICAL LOWER ARM & HORIZONTAL UPPER ARM IN 90 DEGREES #define INITIAL_X 0.0 // CARTESIAN COORDINATE X -#define INITIAL_Y (SHANK_LENGTH+END_EFFECTOR_OFFSET) // CARTESIAN COORDINATE Y -#define INITIAL_Z SHANK_LENGTH // CARTESIAN COORDINATE Z +#define INITIAL_Y (HIGH_SHANK_LENGTH+END_EFFECTOR_OFFSET) // CARTESIAN COORDINATE Y +#define INITIAL_Z LOW_SHANK_LENGTH // CARTESIAN COORDINATE Z #define INITIAL_E0 0.0 // RAIL STEPPER ENDSTOP POSITION // CALIBRATE HOME STEPS TO REACH DESIRED INITIAL_XYZ POSITIONS @@ -81,8 +82,13 @@ //MOVE LIMIT PARAMETERS #define Z_MIN -125.0 //MINIMUM Z HEIGHT OF TOOLHEAD TOUCHING GROUND -#define Z_MAX (SHANK_LENGTH+30.0) //SHANK_LENGTH ADDING ARBITUARY NUMBER FOR Z_MAX -#define R_MIN (SHANK_LENGTH * 0.85 + END_EFFECTOR_OFFSET) //INNER RADIUS MINIMUM. x0.85 APPROX OF LAW OF COSINES -#define R_MAX (SHANK_LENGTH * 1.85 + END_EFFECTOR_OFFSET) //OUTTER RADIUS MAXIMUM. x1.85 APPROX OF LAW OF COSINES +#define Z_MAX (LOW_SHANK_LENGTH+30.0) //SHANK_LENGTH ADDING ARBITUARY NUMBER FOR Z_MAX +#define SHANKS_MIN_ANGLE_COS 0.791436948 +#define SHANKS_MAX_ANGLE_COS -0.774944489 +#define R_MIN (sqrt((sq(LOW_SHANK_LENGTH) + sq(HIGH_SHANK_LENGTH)) - (2*LOW_SHANK_LENGTH*HIGH_SHANK_LENGTH*SHANKS_MIN_ANGLE_COS) )) +#define R_MAX (sqrt((sq(LOW_SHANK_LENGTH) + sq(HIGH_SHANK_LENGTH)) - (2*LOW_SHANK_LENGTH*HIGH_SHANK_LENGTH*SHANKS_MAX_ANGLE_COS) )) + + + #endif diff --git a/arduino_firmware/robotArm_v0.31/interpolation.cpp b/arduino_firmware/robotArm_v0.31/interpolation.cpp index 1724f60..62278a4 100644 --- a/arduino_firmware/robotArm_v0.31/interpolation.cpp +++ b/arduino_firmware/robotArm_v0.31/interpolation.cpp @@ -219,16 +219,22 @@ Point Interpolation::getPosmm() const { } bool Interpolation::isAllowedPosition(float pos_tracker[4]) { - float squaredPositionModule = pos_tracker[X_AXIS]*pos_tracker[X_AXIS] + pos_tracker[Y_AXIS]*pos_tracker[Y_AXIS] + pos_tracker[Z_AXIS]*pos_tracker[Z_AXIS]; - - if((squaredPositionModule <= R_MAX*R_MAX) - && (squaredPositionModule >= R_MIN*R_MIN) - && (pos_tracker[Z_AXIS]) >= Z_MIN - && (pos_tracker[Z_AXIS]) <= Z_MAX - && (pos_tracker[E_AXIS]) <= RAIL_LENGTH) { - return true; - } - //Logger::logDEBUG("squaredPositionModule: " + String(squaredPositionModule) + ", R_MIN^2: " + String(R_MIN*R_MIN) + ", R_MAX^2: " + String(R_MAX*R_MAX)); - Logger::logERROR("LIMIT REACHED: [X:" + String(pos_tracker[X_AXIS]) + " Y:" + String(pos_tracker[Y_AXIS]) + " Z:" + String(pos_tracker[Z_AXIS]) + " E:" + String(pos_tracker[E_AXIS]) + "]"); - return false; + float rrot_ee = hypot(pos_tracker[X_AXIS], pos_tracker[Y_AXIS]); + float rrot = rrot_ee - END_EFFECTOR_OFFSET; + float rrot_x = rrot * (pos_tracker[Y_AXIS] / rrot_ee); + float rrot_y = rrot * (pos_tracker[X_AXIS] / rrot_ee); + float squaredPositionModule = sq(rrot_x) + sq(rrot_y) + sq(pos_tracker[Z_AXIS]); + + bool retVal = ( + squaredPositionModule <= sq(R_MAX) + && squaredPositionModule >= sq(R_MIN) + && pos_tracker[Z_AXIS] >= Z_MIN + && pos_tracker[Z_AXIS] <= Z_MAX + && pos_tracker[E_AXIS] <= RAIL_LENGTH + ); + if(!retVal) { + Logger::logERROR("LIMIT REACHED: [X:" + String(pos_tracker[X_AXIS]) + " Y:" + String(pos_tracker[Y_AXIS]) + " Z:" + String(pos_tracker[Z_AXIS]) + " E:" + String(pos_tracker[E_AXIS]) + "]"); + //Logger::logDEBUG("LIMIT REACHED: [squaredPositionModule:" + String(squaredPositionModule) + " R_MAX^2:" + String(sq(R_MAX)) + " R_MIN^2:" + String(sq(R_MIN)) + " Z_MIN:" + String(Z_MIN) + " Z_MAX:" + String(Z_MAX) + " RAIL_LENGTH:" + String(RAIL_LENGTH) + "]"); + } + return retVal; } diff --git a/arduino_firmware/robotArm_v0.31/robotArm_v0.31.ino b/arduino_firmware/robotArm_v0.31/robotArm_v0.31.ino index 72ba230..153912c 100644 --- a/arduino_firmware/robotArm_v0.31/robotArm_v0.31.ino +++ b/arduino_firmware/robotArm_v0.31/robotArm_v0.31.ino @@ -10,7 +10,7 @@ #include "robotGeometry.h" #include "interpolation.h" #include "fanControl.h" -#include "rampsStepper.h" +#include "RampsStepper.h" #include "queue.h" #include "command.h" #include "byj_gripper.h" @@ -39,7 +39,7 @@ Equipment led(LED_PIN); FanControl fan(FAN_PIN, FAN_DELAY); //EXECUTION & COMMAND OBJECTS -RobotGeometry geometry(END_EFFECTOR_OFFSET, SHANK_LENGTH); +RobotGeometry geometry(END_EFFECTOR_OFFSET, LOW_SHANK_LENGTH, HIGH_SHANK_LENGTH); Interpolation interpolator; Queue queue(QUEUE_SIZE); Command command; diff --git a/arduino_firmware/robotArm_v0.31/robotGeometry.cpp b/arduino_firmware/robotArm_v0.31/robotGeometry.cpp index c70888a..cc6a460 100644 --- a/arduino_firmware/robotArm_v0.31/robotGeometry.cpp +++ b/arduino_firmware/robotArm_v0.31/robotGeometry.cpp @@ -3,9 +3,10 @@ #include #include -RobotGeometry::RobotGeometry(float a_ee_offset, float a_shank_length) { +RobotGeometry::RobotGeometry(float a_ee_offset, float a_low_shank_length, float a_high_shank_length) { ee_offset = a_ee_offset; - shank_length = a_shank_length; + low_shank_length = a_low_shank_length; + high_shank_length = a_high_shank_length; } void RobotGeometry::set(float axmm, float aymm, float azmm) { @@ -41,19 +42,23 @@ float RobotGeometry::getHighRad() const { void RobotGeometry::calculateGrad() { - float rrot = sqrt((xmm * xmm) + (ymm * ymm)) - ee_offset; //radius from Top View - float rrot_ee = rrot + ee_offset; - float rside = sqrt((rrot * rrot) + (zmm * zmm)); //radius from Side View. Use rrot instead of ymm..for everything + float rrot_ee = hypot(xmm, ymm); + float rrot = rrot_ee - ee_offset; //radius from Top View + float rside = hypot(rrot, zmm); //radius from Side View. Use rrot instead of ymm..for everything + float rside_2 = sq(rside); + float low_2 = sq(low_shank_length); + float high_2 = sq(high_shank_length); rot = asin(xmm / rrot_ee); - high = acos((rside * 0.5) / shank_length) * 2.0; + high = PI - acos((low_2 + high_2 - rside_2) / (2 * low_shank_length * high_shank_length)); //Angle of Lower Stepper Motor (asin()=Angle To Gripper) if (zmm > 0) { - low = asin(rrot / rside) + ((PI - high) / 2.0) - (PI / 2.0); + low = acos(zmm / rside) - acos((low_2 - high_2 + rside_2) / (2 * low_shank_length * rside)); } else { - low = PI - asin(rrot / rside) + ((PI - high) / 2.0) - (PI / 2.0); + low = PI - asin(rrot / rside) - acos((low_2 - high_2 + rside_2) / (2 * low_shank_length * rside)); } + //correct higher Angle as it is mechanically bounded width lower Motor high = high + low; } diff --git a/arduino_firmware/robotArm_v0.31/robotGeometry.h b/arduino_firmware/robotArm_v0.31/robotGeometry.h index 6f9013d..2b7d572 100644 --- a/arduino_firmware/robotArm_v0.31/robotGeometry.h +++ b/arduino_firmware/robotArm_v0.31/robotGeometry.h @@ -3,7 +3,7 @@ class RobotGeometry { public: - RobotGeometry(float a_ee_offset, float a_shank_length); + RobotGeometry(float a_ee_offset, float a_low_shank_length, float a_high_length); void set(float axmm, float aymm, float azmm); float getXmm() const; float getYmm() const; @@ -14,7 +14,8 @@ class RobotGeometry { private: void calculateGrad(); float ee_offset; - float shank_length; + float low_shank_length; + float high_shank_length; float xmm; float ymm; float zmm;