Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Different shanks length support; Improved position validation; #3

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Different shanks length support; Improved position validation; #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion arduino_firmware/robotArm_v0.31/RampsStepper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
#include "rampsStepper.h"
#include "RampsStepper.h"
#include "config.h"

#include <Arduino.h>
Expand Down
18 changes: 12 additions & 6 deletions arduino_firmware/robotArm_v0.31/config.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
30 changes: 18 additions & 12 deletions arduino_firmware/robotArm_v0.31/interpolation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
4 changes: 2 additions & 2 deletions arduino_firmware/robotArm_v0.31/robotArm_v0.31.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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<Cmd> queue(QUEUE_SIZE);
Command command;
Expand Down
21 changes: 13 additions & 8 deletions arduino_firmware/robotArm_v0.31/robotGeometry.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,9 +3,10 @@
#include <math.h>
#include <Arduino.h>

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) {
Expand Down Expand Up @@ -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;
}
5 changes: 3 additions & 2 deletions arduino_firmware/robotArm_v0.31/robotGeometry.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand Down