diff --git a/README.md b/README.md index 803b784..b595099 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,10 @@ # community_robot_arm **REPOSITORY OF COMMUNITY ROBOT ARM by 20sffactory** +* arduino_firmware Version V0.62 (07Jun2021) + - Change position format in M114 + - Fixed Issue of G28 Homing with endstop for to detect failure robot + - Change flow on homeSequence. Plus generality of different hardware * arduino_firmware Version V0.61 (03May2021) - Arduino Uno Controller Option - Fixed Issue of G28 Homing for Original Ftobler without endstop diff --git a/arduino_firmware/robotArm_v0.62/RampsStepper.cpp b/arduino_firmware/robotArm_v0.62/RampsStepper.cpp new file mode 100644 index 0000000..a37dc1c --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/RampsStepper.cpp @@ -0,0 +1,84 @@ +#include "RampsStepper.h" +#include "config.h" + +#include + +RampsStepper::RampsStepper(int aStepPin, int aDirPin, int aEnablePin, bool aInverse) { + setReductionRatio(MAIN_GEAR_TEETH / MOTOR_GEAR_TEETH, MICROSTEPS * STEPS_PER_REV); + stepPin = aStepPin; + dirPin = aDirPin; + enablePin = aEnablePin; + inverse = aInverse; + stepperStepPosition = 0; + stepperStepTargetPosition; + pinMode(stepPin, OUTPUT); + pinMode(dirPin, OUTPUT); + pinMode(enablePin, OUTPUT); + enable(false); +} + +void RampsStepper::enable(bool value) { + digitalWrite(enablePin, !value); +} + +bool RampsStepper::isOnPosition() const { + return stepperStepPosition == stepperStepTargetPosition; +} + +int RampsStepper::getPosition() const { + return stepperStepPosition; +} + +void RampsStepper::setPosition(int value) { + stepperStepPosition = value; + stepperStepTargetPosition = value; +} + +void RampsStepper::stepToPosition(int value) { + stepperStepTargetPosition = value; +} + +void RampsStepper::stepToPositionMM(float mm, float steps_per_mm) { + stepperStepTargetPosition = mm * steps_per_mm; +} + +void RampsStepper::stepRelative(int value) { + value += stepperStepPosition; + stepToPosition(value); +} + +float RampsStepper::getPositionRad() const { + return stepperStepPosition / radToStepFactor; +} + +void RampsStepper::setPositionRad(float rad) { + setPosition(rad * radToStepFactor); +} + +void RampsStepper::stepToPositionRad(float rad) { + stepperStepTargetPosition = rad * radToStepFactor; +} + +void RampsStepper::stepRelativeRad(float rad) { + stepRelative(rad * radToStepFactor); +} + +void RampsStepper::update() { + while (stepperStepTargetPosition < stepperStepPosition) { + digitalWrite(dirPin, !inverse); + digitalWrite(stepPin, HIGH); + digitalWrite(stepPin, LOW); + stepperStepPosition--; + } + + while (stepperStepTargetPosition > stepperStepPosition) { + digitalWrite(dirPin, inverse); + digitalWrite(stepPin, HIGH); + digitalWrite(stepPin, LOW); + stepperStepPosition++; + } +} + +void RampsStepper::setReductionRatio(float gearRatio, int stepsPerRev) { + radToStepFactor = gearRatio * stepsPerRev / 2 / PI; +}; diff --git a/arduino_firmware/robotArm_v0.62/RampsStepper.h b/arduino_firmware/robotArm_v0.62/RampsStepper.h new file mode 100644 index 0000000..0dd0000 --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/RampsStepper.h @@ -0,0 +1,33 @@ +#ifndef RAMPSSTEPPER_H_ +#define RAMPSSTEPPER_H_ + +class RampsStepper { +public: + RampsStepper(int aStepPin, int aDirPin, int aEnablePin, bool aInverse); + void enable(bool value = true); + + bool isOnPosition() const; + int getPosition() const; + void setPosition(int value); + void stepToPosition(int value); + void stepToPositionMM(float mm, float steps_per_mm); + void stepRelative(int value); + float getPositionRad() const; + void setPositionRad(float rad); + void stepToPositionRad(float rad); + void stepRelativeRad(float rad); + + void update(); + + void setReductionRatio(float gearRatio, int stepsPerRev); +private: + int stepperStepTargetPosition; + int stepperStepPosition; + int stepPin; + int dirPin; + int enablePin; + bool inverse; + float radToStepFactor; +}; + +#endif diff --git a/arduino_firmware/robotArm_v0.62/byj_gripper.cpp b/arduino_firmware/robotArm_v0.62/byj_gripper.cpp new file mode 100644 index 0000000..5cb0f35 --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/byj_gripper.cpp @@ -0,0 +1,106 @@ +#include "byj_gripper.h" +#include + +BYJ_Gripper::BYJ_Gripper(int pin0, int pin1, int pin2, int pin3, int steps){ + grip_steps = steps; + byj_pin_0 = pin0; + byj_pin_1 = pin1; + byj_pin_2 = pin2; + byj_pin_3 = pin3; + step_cycle = 0; + pinMode(byj_pin_0, OUTPUT); + pinMode(byj_pin_1, OUTPUT); + pinMode(byj_pin_2, OUTPUT); + pinMode(byj_pin_3, OUTPUT); +} + +void BYJ_Gripper::cmdOn() { + direction = true; + for (int i = 1; i <= grip_steps; i++) { + moveSteps(); + delay(1); + } +} + +void BYJ_Gripper::cmdOff() { + direction = false; + for (int i = 1; i <= grip_steps; i++) { + moveSteps(); + delay(1); + } +} + +void BYJ_Gripper::setDirection(){ + if (direction == true) { + step_cycle++; + } + if (direction == false) { + step_cycle--; + } + if (step_cycle > 7) { + step_cycle = 0; + } + if (step_cycle < 0) { + step_cycle = 7; + } +} + +void BYJ_Gripper::moveSteps() { + switch (step_cycle) { + case 0: + digitalWrite(byj_pin_0, LOW); + digitalWrite(byj_pin_1, LOW); + digitalWrite(byj_pin_2, LOW); + digitalWrite(byj_pin_3, HIGH); + break; + case 1: + digitalWrite(byj_pin_0, LOW); + digitalWrite(byj_pin_1, LOW); + digitalWrite(byj_pin_2, HIGH); + digitalWrite(byj_pin_3, HIGH); + break; + case 2: + digitalWrite(byj_pin_0, LOW); + digitalWrite(byj_pin_1, LOW); + digitalWrite(byj_pin_2, HIGH); + digitalWrite(byj_pin_3, LOW); + break; + case 3: + digitalWrite(byj_pin_0, LOW); + digitalWrite(byj_pin_1, HIGH); + digitalWrite(byj_pin_2, HIGH); + digitalWrite(byj_pin_3, LOW); + break; + case 4: + digitalWrite(byj_pin_0, LOW); + digitalWrite(byj_pin_1, HIGH); + digitalWrite(byj_pin_2, LOW); + digitalWrite(byj_pin_3, LOW); + break; + case 5: + digitalWrite(byj_pin_0, HIGH); + digitalWrite(byj_pin_1, HIGH); + digitalWrite(byj_pin_2, LOW); + digitalWrite(byj_pin_3, LOW); + break; + case 6: + digitalWrite(byj_pin_0, HIGH); + digitalWrite(byj_pin_1, LOW); + digitalWrite(byj_pin_2, LOW); + digitalWrite(byj_pin_3, LOW); + break; + case 7: + digitalWrite(byj_pin_0, HIGH); + digitalWrite(byj_pin_1, LOW); + digitalWrite(byj_pin_2, LOW); + digitalWrite(byj_pin_3, HIGH); + break; + default: + digitalWrite(byj_pin_0, LOW); + digitalWrite(byj_pin_1, LOW); + digitalWrite(byj_pin_2, LOW); + digitalWrite(byj_pin_3, LOW); + break; + } + setDirection(); +} diff --git a/arduino_firmware/robotArm_v0.62/byj_gripper.h b/arduino_firmware/robotArm_v0.62/byj_gripper.h new file mode 100644 index 0000000..07c9c58 --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/byj_gripper.h @@ -0,0 +1,21 @@ +#ifndef BYJ_GRIPPER_H_ +#define BYJ_GRIPPER_H_ + + +class BYJ_Gripper { +public: + BYJ_Gripper(int pin0, int pin1, int pin2, int pin3, int steps); + void cmdOn(); + void cmdOff(); +private: + bool direction; + void moveSteps(); + void setDirection(); + int byj_pin_0; + int byj_pin_1; + int byj_pin_2; + int byj_pin_3; + int grip_steps; + int step_cycle; +}; +#endif diff --git a/arduino_firmware/robotArm_v0.62/command.cpp b/arduino_firmware/robotArm_v0.62/command.cpp new file mode 100644 index 0000000..a4d084c --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/command.cpp @@ -0,0 +1,132 @@ +#include "command.h" +#include "logger.h" +#include "config.h" +#include + +Command::Command() { + //initialize Command to a zero-move value; + new_command.valueX = NAN; + new_command.valueY = NAN; + new_command.valueZ = NAN; + new_command.valueF = 0; + new_command.valueE = NAN; + new_command.valueS = 0; + message = ""; + isRelativeCoord = false; +} + +bool Command::handleGcode() { + if (Serial.available()) { + char c = Serial.read(); + if (c == '\n') { + return false; + } + if (c == '\r') { + bool b = processMessage(message); + message = ""; + return b; + } else { + message += c; + } + } + return false; +} + +bool Command::processMessage(String msg){ + + new_command.valueX = NAN; + new_command.valueY = NAN; + new_command.valueZ = NAN; + new_command.valueE = NAN; + new_command.valueF = 0; + new_command.valueS = 0; + msg.toUpperCase(); + msg.replace(" ", ""); + int active_index = 0; + new_command.id = msg[active_index]; + if((new_command.id != 'G') && (new_command.id != 'M')){ + printErr(); + return false; + } + + active_index++; + int temp_index = active_index; + while (temp_index +#include "interpolation.h" + +struct Cmd { + char id; + int num; + float valueX; + float valueY; + float valueZ; + float valueF; + float valueE; + float valueS; +}; + +class Command { + public: + Command(); + bool handleGcode(); + bool processMessage(String msg); + void value_segment(String msg_segment); + Cmd getCmd() const; + void cmdGetPosition(Point pos, Point pos_offset, float highRad, float lowRad, float rotRad); + void cmdToRelative(); + void cmdToAbsolute(); + bool isRelativeCoord; + Cmd new_command; + + private: + String message; +}; + +void cmdMove(Cmd(&cmd), Point pos, Point pos_offset, bool isRelativeCoord); +void cmdDwell(Cmd(&cmd)); +void printErr(); + +#endif diff --git a/arduino_firmware/robotArm_v0.62/config.h b/arduino_firmware/robotArm_v0.62/config.h new file mode 100644 index 0000000..4ae6de5 --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/config.h @@ -0,0 +1,113 @@ +#ifndef CONFIG_H_ +#define CONFIG_H_ + +//SERIAL SETTINGS +#define BAUD 115200 + +//MEGA2560 BY DEFAULT, SET TO true IF UNO & CNC SHILED USED TO DRIVE ROBOT +#define USE_UNO false + +//ROBOT ARM LENGTH +//#define SHANK_LENGTH 140.0 +#define LOW_SHANK_LENGTH 120.0 +#define HIGH_SHANK_LENGTH 120.0 + +#define END_EFFECTOR_OFFSET 54.0 // LENGTH FROM UPPER SHANK BEARING TO MIDPOINT OF END EFFECTOR IN MM +#define END_EFFECTOR_OFFSET 50.0 // FOR GRIPPER PUMA3D + +//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 (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 +#define X_HOME_STEPS 665 //765 //860 // STEPS FROM X_ENDSTOP TO INITIAL_XYZ FOR UPPER ARM +#define Y_HOME_STEPS 1390 //1940 // STEPS FROM Y_ENDSTOP TO INITIAL_XYZ FOR LOWER ARM +#define Z_HOME_STEPS 2450 // STEPS FROM Z_ENDSTOP TO INITIAL_XYZ FOR ROTATION CENTER +#define E0_HOME_STEPS 0 // STEPS FROM E0_ENDSTOP TO INITIAL_E0 + +//HOMING SETTINGS: +#define HOME_X_STEPPER true // "true" IF ENDSTOP IS INSTALLED +#define HOME_Y_STEPPER true // "true" IF ENDSTOP IS INSTALLED +#define HOME_Z_STEPPER true // "true" IF ENDSTOP IS INSTALLED +#define HOME_E0_STEPPER false // "true" IF ENDSTOP IS INSTALLED +#define HOME_ON_BOOT false // "true" IF HOMING REQUIRED AFTER POWER ON +#define HOME_DWELL 1400 // INCREASE TO SLOW DOWN HOMING SPEED + +//STEPPER SETTINGS: +#define MICROSTEPS 16 // MICROSTEPPING CONFIGURATION ON RAMPS1.4 +#define STEPS_PER_REV 200 // NEMA17 STEPS PER REVOLUTION +#define INVERSE_X_STEPPER false // CHANGE IF STEPPER MOVES OTHER WAY +#define INVERSE_Y_STEPPER false // CHANGE IF STEPPER MOVES OTHER WAY +#define INVERSE_Z_STEPPER true // CHANGE IF STEPPER MOVES OTHER WAY +#define INVERSE_E0_STEPPER false // CHANGE IF STEPPER MOVES OTHER WAY + +//RAIL SETTINGS: +#define RAIL false // E0 STEPPER USED AS RAIL. SET TO 'false' IF ROBOT ARM IS STATIONARY. +#define STEPS_PER_MM_RAIL 80.0 // STEPS PER MM FOR RAIL MOTOR +#define RAIL_LENGTH 200.0 // MAX LENGTH OF RAIL IN MM + +//ENDSTOP SETTINGS: +#define X_MIN_INPUT 0 // OUTPUT VALUE WHEN SWITCH ACTIVATED +#define Y_MIN_INPUT 0 // OUTPUT VALUE WHEN SWITCH ACTIVATED +#define Z_MIN_INPUT 0 // OUTPUT VALUE WHEN SWITCH ACTIVATED +#define E0_MIN_INPUT 0 // OUTPUT VALUE WHEN SWITCH ACTIVATED + +//GEAR RATIO SETTINGS +#define MOTOR_GEAR_TEETH 9.0 // 20.0 FOR 20SFFACTORY BELT VERSION 9.0 FOR FTOBLER GEAR VERSION +#define MAIN_GEAR_TEETH 32.0 // 90.0 FOR 20SFFACTORY BELT VERSION 32.0 FOR FTOBLER GEAR VERSION + +//EQUIPMENT SETTINGS +#define LASER false // 12V LASER CONNECTED TO LASER_PIN +#define PUMP false // 12V AIR PUMP CONNECTED TO PUMP_PIN +#define FAN_DELAY 120 // FAN ON IN SECONDS + +//GRIPPER SETTINGS +#define GRIPPER 2 //GRIPPER MOTOR IN USE + // 0: 28BYJ-48 MICRO STEPPER MOTOR + // 1: 9G SERVO OR MG996 SERVO EQUIVALENT + // 2: 28BYJ-48 HACKED STEPPER MOTOR WITH A4988 ON E1 +//28BYJ GRIPPER SETTINGS +#define BYJ_GRIP_STEPS 1200 //FTOBLER: 1200 +//SERVO GRIPPER SETTINGS +#define SERVO_GRIP_DEGREE 90.0 +#define SERVO_UNGRIP_DEGREE 0.0 + +//COMMAND QUEUE SETTINGS +#define QUEUE_SIZE 15 + +//PRINT REPLY SETTING +#define PRINT_REPLY true // "true" TO PRINT MSG AFTER ONE COMMAND IS PROCESSED +#define PRINT_REPLY_MSG "ok" // MSG SENT FOR USER'S POST PROCESSING WITH OTHER SOFTWARE + +//SPEED PROFILE SETTING +#define SPEED_PROFILE 2 // OPTIONS BELOW +//0: FLAT SPEED CURVE (CONSTANT SPEED PER MOVEMENT, SUITABLE FOR REALTIME CONTROL SOFTWARE) +//1: ARCTAN APPROX (SLIGHT BELL CURVE ACCELERATION & DECELERATION) +//2: COSIN APPROX (TOTAL BELL CURVE ACCEL FROM 0 & DECEL FROM 0, SUITABLE FOR PRESET COMMAND MOVEMENTS) + + +//LOG SETTINGS +#define LOG_LEVEL 2 +//0: ERROR +//1: INFO +//2: DEBUG + +//MOVE LIMIT PARAMETERS +#define Z_MIN -88 // -140.0 //MINIMUM Z HEIGHT OF TOOLHEAD TOUCHING GROUND +#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) )) + +//ENDSTOPS CHECK. DEFAULT DELAYS ON HOME_DWELL = 1400 +#define X_CHECK_DELAY 4 // WAITING TIME TO ASSUME LACK OF POWER OR ELECTROMECHANICAL FAILURE (default 4 seconds) +#define Y_CHECK_DELAY 7 // WAITING TIME TO ASSUME LACK OF POWER OR ELECTROMECHANICAL FAILURE (default 7 seconds) +#define Z_CHECK_DELAY 7 // WAITING TIME TO ASSUME LACK OF POWER OR ELECTROMECHANICAL FAILURE (default 7 seconds) +#define E0_CHECK_DELAY 3 // WAITING TIME TO ASSUME LACK OF POWER OR ELECTROMECHANICAL FAILURE (default 3 seconds) + +#endif diff --git a/arduino_firmware/robotArm_v0.62/endstop.cpp b/arduino_firmware/robotArm_v0.62/endstop.cpp new file mode 100644 index 0000000..f354e56 --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/endstop.cpp @@ -0,0 +1,102 @@ +#include "endstop.h" +#include + +Endstop::Endstop(int a_min_pin, int a_dir_pin, int a_step_pin, int a_en_pin, int a_switch_input, int a_step_offset, int a_home_dwell, int a_check_delay){ + min_pin = a_min_pin; + dir_pin = a_dir_pin; + step_pin = a_step_pin; + en_pin = a_en_pin; + switch_input = a_switch_input; + home_dwell = a_home_dwell; + step_offset = a_step_offset; + check_delay = a_check_delay; + bCheckDelay = false; + pinMode(min_pin, INPUT_PULLUP); +} + +void Endstop::home(bool dir) { + digitalWrite(en_pin, LOW); + delayMicroseconds(5); + if (dir==1){ + digitalWrite(dir_pin, HIGH); + } else { + digitalWrite(dir_pin, LOW); + } + delayMicroseconds(5); + + // analiza si en un tiempo dado los endstops cambian de estado + // si no: fallo electromecanico + bCheckDelay = false; + long cDelay = 0; +/* en v51 + bState = digitalRead(min_pin); + while (bState != switch_input) { + digitalWrite(step_pin, HIGH); + digitalWrite(step_pin, LOW); + delayMicroseconds(home_dwell); + bState = digitalRead(min_pin); + cDelay += home_dwell; + if(cDelay/1000000 > check_delay and bState != switch_input){ + bCheckDelay = true; + break; + } + } + homeOffset(dir); +*/ + bState = !(digitalRead(min_pin) ^ switch_input); + while (!bState) { + digitalWrite(step_pin, HIGH); + digitalWrite(step_pin, LOW); + delayMicroseconds(home_dwell); + bState = !(digitalRead(min_pin) ^ switch_input); + cDelay += home_dwell; + if(!bState and cDelay/1000000 > check_delay){ + bCheckDelay = true; + break; + } + } + homeOffset(dir); +} + +void Endstop::homeOffset(bool dir){ + if (dir==1){ + digitalWrite(dir_pin, LOW); + } + else{ + digitalWrite(dir_pin, HIGH); + } + delayMicroseconds(5); + for (int i = 1; i <= step_offset; i++) { + digitalWrite(step_pin, HIGH); + digitalWrite(step_pin, LOW); + delayMicroseconds(home_dwell); + } +} + +void Endstop::oneStepToEndstop(bool dir){ + digitalWrite(en_pin, LOW); + delayMicroseconds(5); + if (dir==1){ + digitalWrite(dir_pin, HIGH); + } else { + digitalWrite(dir_pin, LOW); + } + delayMicroseconds(5); + bState = !(digitalRead(min_pin) ^ switch_input); + + if (!bState) { + digitalWrite(step_pin, HIGH); + digitalWrite(step_pin, LOW); + delayMicroseconds(home_dwell); + } + bState = !(digitalRead(min_pin) ^ switch_input); +} + +bool Endstop::state(){ + bState = !(digitalRead(min_pin) ^ switch_input); + return bState; +} + +bool Endstop::checkDelay(){ + return bCheckDelay; +} diff --git a/arduino_firmware/robotArm_v0.62/endstop.h b/arduino_firmware/robotArm_v0.62/endstop.h new file mode 100644 index 0000000..a2c32b9 --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/endstop.h @@ -0,0 +1,26 @@ +#ifndef ENDSTOP_H_ +#define ENDSTOP_H_ + +class Endstop { + public: + Endstop(int a_min_pin, int a_dir_pin, int a_step_pin, int a_en_pin, int a_switch_input, int a_step_offset, int a_home_dwell, int a_check_delay); + void home(bool dir); + void homeOffset(bool dir); + void oneStepToEndstop(bool dir); + bool state(); + bool checkDelay(); + + private: + int min_pin; + int dir_pin; + int step_pin; + int en_pin; + int switch_input; + int home_dwell; + int step_offset; + bool bState; + int check_delay; // seconds + bool bCheckDelay; +}; + +#endif diff --git a/arduino_firmware/robotArm_v0.62/equipment.cpp b/arduino_firmware/robotArm_v0.62/equipment.cpp new file mode 100644 index 0000000..bd3e6e6 --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/equipment.cpp @@ -0,0 +1,15 @@ +#include "equipment.h" +#include + +Equipment::Equipment(int equipment_pin){ + pin = equipment_pin; + pinMode(pin, OUTPUT); +} + +void Equipment::cmdOn(){ + digitalWrite(pin, HIGH); +} + +void Equipment::cmdOff(){ + digitalWrite(pin, LOW); +} diff --git a/arduino_firmware/robotArm_v0.62/equipment.h b/arduino_firmware/robotArm_v0.62/equipment.h new file mode 100644 index 0000000..b698eaf --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/equipment.h @@ -0,0 +1,13 @@ +#ifndef EQUIPMENT_H_ +#define EQUIPMENT_H_ + +class Equipment { +public: + Equipment(int equipment_pin); + void cmdOn(); + void cmdOff(); +private: + int pin; +}; + +#endif diff --git a/arduino_firmware/robotArm_v0.62/fanControl.cpp b/arduino_firmware/robotArm_v0.62/fanControl.cpp new file mode 100644 index 0000000..b800cb4 --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/fanControl.cpp @@ -0,0 +1,35 @@ +#include "fanControl.h" +#include + + +FanControl::FanControl(int aPin, int aFanDelay) { + fan_delay = aFanDelay * 1000; + nextShutdown = 0; + pin = aPin; + pinMode(pin , OUTPUT); + digitalWrite(pin , LOW); + state = false; +} + +void FanControl::enable(bool value) { + if (value) { + state = true; + digitalWrite(pin, HIGH); + } else { + disable(); + } +} + +void FanControl::disable() { + state = false; + nextShutdown = millis() + fan_delay; + update(); +} + +void FanControl::update() { + if (!state) { + if (millis() >= nextShutdown) { + digitalWrite(pin, LOW); + } + } +} diff --git a/arduino_firmware/robotArm_v0.62/fanControl.h b/arduino_firmware/robotArm_v0.62/fanControl.h new file mode 100644 index 0000000..c6248d5 --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/fanControl.h @@ -0,0 +1,17 @@ +#ifndef FANCONTROL_H_ +#define FANCONTROL_H_ + +class FanControl { +public: + FanControl(int aPin, int aFanDelay); + void enable(bool value = true); + void disable(); + void update(); +private: + bool state; + int pin; + long fan_delay; + long nextShutdown; +}; + +#endif diff --git a/arduino_firmware/robotArm_v0.62/interpolation.cpp b/arduino_firmware/robotArm_v0.62/interpolation.cpp new file mode 100644 index 0000000..a9a63ab --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/interpolation.cpp @@ -0,0 +1,239 @@ +#include "interpolation.h" +#include "config.h" +#include "queue.h" +#include "logger.h" + +Interpolation::Interpolation(){ + pos_offset.xmm = 0.0; + pos_offset.ymm = 0.0; + pos_offset.zmm = 0.0; + pos_offset.emm = 0.0; +} + +//G92 POSITION OFFSET FUNCTIONS +void Interpolation::setPosOffset(float new_x, float new_y, float new_z, float new_e) { + pos_offset.xmm = xPosmm - new_x; + pos_offset.ymm = yPosmm - new_y; + pos_offset.zmm = zPosmm - new_z; + pos_offset.emm = ePosmm - new_e; + Logger::logINFO("POSITION OFFSET: [X" + String(pos_offset.xmm) + " Y:" + String(pos_offset.ymm) + " Z:" + String(pos_offset.zmm) + " E:" + String(pos_offset.emm) + "]"); + Logger::logINFO("CURRENT POSITION: [X:"+String(new_x)+" Y:"+String(new_y)+" Z:"+String(new_z)+" E:"+String(new_e)+"]"); +} + +void Interpolation::resetPosOffset(){ + pos_offset.xmm = 0.0; + pos_offset.ymm = 0.0; + pos_offset.zmm = 0.0; + pos_offset.emm = 0.0; +} + +Point Interpolation::getPosOffset() const { + return pos_offset; +} + + +void Interpolation::setCurrentPos(float px, float py, float pz, float pe) { + Point p; + p.xmm = px; + p.ymm = py; + p.zmm = pz; + p.emm = pe; + setCurrentPos(p); +} + +void Interpolation::setInterpolation(float px, float py, float pz, float pe, float v) { + Point p; + p.xmm = px; + p.ymm = py; + p.zmm = pz; + p.emm = pe; + setInterpolation(p, v); +} + +void Interpolation::setInterpolation(float p1x, float p1y, float p1z, float p1e, float p2x, float p2y, float p2z, float p2e, float v) { + Point p1; + Point p2; + p1.xmm = p1x; + p1.ymm = p1y; + p1.zmm = p1z; + p1.emm = p1e; + p2.xmm = p2x; + p2.ymm = p2y; + p2.zmm = p2z; + p2.emm = p2e; + setInterpolation(p1, p2, v); +} + +void Interpolation::setInterpolation(Point p1, float v) { + Point p0; + p0.xmm = xStartmm + xDelta; + p0.ymm = yStartmm + yDelta; + p0.zmm = zStartmm + zDelta; + p0.emm = eStartmm + eDelta; + setInterpolation(p0, p1, v); +} + +void Interpolation::setInterpolation(Point p0, Point p1, float av) { + v = av; //mm/s + + float a = (p1.xmm - p0.xmm); + float b = (p1.ymm - p0.ymm); + float c = (p1.zmm - p0.zmm); + float e = abs(p1.emm - p0.emm); + float dist = sqrt(a*a + b*b + c*c); + + if (dist < e) { + dist = e; + } + + if (v < 5) { //includes 0 = default value + v = sqrt(dist) * 10; //set a good value for v + } + if (v < 5) { + v = 5; + } + + tmul = v / dist; + + xStartmm = p0.xmm; + yStartmm = p0.ymm; + zStartmm = p0.zmm; + eStartmm = p0.emm; + + xDelta = (p1.xmm - p0.xmm); + yDelta = (p1.ymm - p0.ymm); + zDelta = (p1.zmm - p0.zmm); + eDelta = (p1.emm - p0.emm); + + state = 0; + + startTime = micros(); +} + +void Interpolation::setCurrentPos(Point p) { + xStartmm = p.xmm; + yStartmm = p.ymm; + zStartmm = p.zmm; + eStartmm = p.emm; + xDelta = 0; + yDelta = 0; + zDelta = 0; + eDelta = 0; +} + +void Interpolation::updateActualPosition() { + if (state != 0) { + return; + } + long microsek = micros(); + float t = (microsek - startTime) / 1000000.0; + float progress; + switch (SPEED_PROFILE){ + // FLAT SPEED CURVE + case 0: + progress = t * tmul; + if (progress >= 1.0){ + progress = 1.0; + state = 1; + } + break; + // ARCTAN APPROX + case 1: + progress = atan((PI * t * tmul) - (PI * 0.5)) * 0.5 + 0.5; + if (progress >= 1.0) { + progress = 1.0; + state = 1; + } + break; + // COSIN APPROX + case 2: + progress = -cos(t * tmul * PI) * 0.5 + 0.5; + if ((t * tmul) >= 1.0) { + progress = 1.0; + state = 1; + } + break; + } + pos_tracker[X_AXIS] = xStartmm + progress * xDelta; + pos_tracker[Y_AXIS] = yStartmm + progress * yDelta; + pos_tracker[Z_AXIS] = zStartmm + progress * zDelta; + pos_tracker[E_AXIS] = eStartmm + progress * eDelta; + + if(isAllowedPosition(pos_tracker)){ + xPosmm = pos_tracker[X_AXIS]; + yPosmm = pos_tracker[Y_AXIS]; + zPosmm = pos_tracker[Z_AXIS]; + ePosmm = pos_tracker[E_AXIS]; + } else { + pos_tracker[X_AXIS] = xPosmm; + pos_tracker[Y_AXIS] = yPosmm; + pos_tracker[Z_AXIS] = zPosmm; + pos_tracker[E_AXIS] = ePosmm; + state = 1; + progress = 1.0; + xStartmm = xPosmm; + yStartmm = yPosmm; + zStartmm = zPosmm; + eStartmm = ePosmm; + xDelta = 0; + yDelta = 0; + zDelta = 0; + eDelta = 0; + } + //FOR DECIPHERING SPEED CURVE + //Serial.print("xPosmm:"); + //Serial.print(xPosmm); + //Serial.print(" yPosmm:"); + //Serial.print(yPosmm); + //Serial.print(" zPosmm:"); + //Serial.println(zPosmm); +} + +bool Interpolation::isFinished() const { + return state != 0; +} + +float Interpolation::getXPosmm() const { + return xPosmm; +} + +float Interpolation::getYPosmm() const { + return yPosmm; +} + +float Interpolation::getZPosmm() const { + return zPosmm; +} + +float Interpolation::getEPosmm() const { + return ePosmm; +} + +Point Interpolation::getPosmm() const { + Point p; + p.xmm = xPosmm; + p.ymm = yPosmm; + p.zmm = zPosmm; + p.emm = ePosmm; + return p; +} + +bool Interpolation::isAllowedPosition(float pos_tracker[4]) { + 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]) + "]"); + } + return retVal; +} diff --git a/arduino_firmware/robotArm_v0.62/interpolation.h b/arduino_firmware/robotArm_v0.62/interpolation.h new file mode 100644 index 0000000..7d808b1 --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/interpolation.h @@ -0,0 +1,66 @@ +#ifndef INTERPOLATION_H_ +#define INTERPOLATION_H_ +#include + +#define X_AXIS 0 +#define Y_AXIS 1 +#define Z_AXIS 2 +#define E_AXIS 3 + +struct Point { + float xmm; + float ymm; + float zmm; + float emm; +}; +class Interpolation { +public: + //void resetInterpolation(float px, float py, float pz); + //void resetInterpolation(float p1x, float p1y, float p1z, float p2x, float p2y, float p2z); + //void resetInterpolation(Point p0, Point p1); + Interpolation(); + void setCurrentPos(float px, float py, float pz, float pe); + void setInterpolation(float px, float py, float pz, float pe, float v = 0); + void setInterpolation(float p1x, float p1y, float p1z, float p1e, float p2x, float p2y, float p2z, float p2e, float av = 0); + + void setCurrentPos(Point p); + void setInterpolation(Point p1, float v = 0); + void setInterpolation(Point p0, Point p1, float v = 0); + + void updateActualPosition(); + bool isFinished() const; + + float getXPosmm() const; + float getYPosmm() const; + float getZPosmm() const; + float getEPosmm() const; + Point getPosmm() const; + bool isAllowedPosition(float pos_tracker[4]); + void setPosOffset(float new_x, float new_y, float new_z, float new_e); + void resetPosOffset(); + Point getPosOffset() const; + +private: + Point pos_offset; + float pos_tracker[4]; + byte state; + + long startTime; + + float xStartmm; + float yStartmm; + float zStartmm; + float eStartmm; + float xDelta; + float yDelta; + float zDelta; + float eDelta; + float xPosmm; + float yPosmm; + float zPosmm; + float ePosmm; + float v; + float tmul; +}; + +#endif diff --git a/arduino_firmware/robotArm_v0.62/logger.cpp b/arduino_firmware/robotArm_v0.62/logger.cpp new file mode 100644 index 0000000..08ee547 --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/logger.cpp @@ -0,0 +1,30 @@ +#include "logger.h" +#include "config.h" + +void Logger::log(String message, int level) { + if(LOG_LEVEL >= level) { + String logMsg; + switch(level) { + case LOG_ERROR: + logMsg = "ERROR: "; + break; + case LOG_INFO: + logMsg = "INFO: "; + break; + case LOG_DEBUG: + logMsg = "DEBUG: "; + break; + } + logMsg = logMsg + message; + Serial.println(logMsg); + } +} +void Logger::logERROR(String message) { + log(message, LOG_ERROR); +} +void Logger::logINFO(String message) { + log(message, LOG_INFO); +} +void Logger::logDEBUG(String message) { + log(message, LOG_DEBUG); +} \ No newline at end of file diff --git a/arduino_firmware/robotArm_v0.62/logger.h b/arduino_firmware/robotArm_v0.62/logger.h new file mode 100644 index 0000000..ce41744 --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/logger.h @@ -0,0 +1,17 @@ +#ifndef LOGGER_H_ +#define LOGGER_H_ + +#include + +#define LOG_ERROR 0 +#define LOG_INFO 1 +#define LOG_DEBUG 2 + +class Logger { + public: + static void log(String message, int level); + static void logINFO(String message); + static void logERROR(String message); + static void logDEBUG(String message); +}; +#endif diff --git a/arduino_firmware/robotArm_v0.62/pinout.h b/arduino_firmware/robotArm_v0.62/pinout.h new file mode 100644 index 0000000..ea5cfa9 --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/pinout.h @@ -0,0 +1,65 @@ +#ifndef PINOUT_H_ +#define PINOUT_H_ + +/* + * pinout of RAMPS 1.4 + * + * source: http://reprap.org/wiki/RAMPS_1.4 + */ + +//RAMPS 1.4 PINS +#define X_STEP_PIN 54 +#define X_DIR_PIN 55 +#define X_ENABLE_PIN 38 +#define X_MIN_PIN 3 +#define X_MAX_PIN 2 + +#define Y_STEP_PIN 60 +#define Y_DIR_PIN 61 +#define Y_ENABLE_PIN 56 +#define Y_MIN_PIN 14 +#define Y_MAX_PIN 15 + +#define Z_STEP_PIN 46 +#define Z_DIR_PIN 48 +#define Z_ENABLE_PIN 62 +#define Z_MIN_PIN 18 +#define Z_MAX_PIN 19 + +#define E0_STEP_PIN 26 +#define E0_DIR_PIN 28 +#define E0_ENABLE_PIN 24 +#define E0_MIN_PIN 20 + +#define E1_STEP_PIN 36 +#define E1_DIR_PIN 34 +#define E1_ENABLE_PIN 30 + +#define BYJ_PIN_0 40 +#define BYJ_PIN_1 63 +#define BYJ_PIN_2 59 +#define BYJ_PIN_3 64 + +#define SERVO_PIN 4 + +#define PUMP_PIN 8 +#define LASER_PIN 10 +#define LED_PIN 13 + +#define SDPOWER -1 +#define SDSS 53 + +#define FAN_PIN 9 + +#define PS_ON_PIN 12 +#define KILL_PIN -1 + +//#define HEATER_0_PIN 10 +//#define HEATER_1_PIN 8 +#define TEMP_0_PIN 13 // ANALOG NUMBERING +#define TEMP_1_PIN 14 // ANALOG NUMBERING + +//RAMPS AUX-2 + + +#endif diff --git a/arduino_firmware/robotArm_v0.62/pinout_uno.h b/arduino_firmware/robotArm_v0.62/pinout_uno.h new file mode 100644 index 0000000..db08581 --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/pinout_uno.h @@ -0,0 +1,53 @@ +#ifndef PINOUT_UNO_H_ +#define PINOUT_UNO_H_ + +#define X_STEP_PIN 2 +#define X_DIR_PIN 5 +#define X_ENABLE_PIN 8 +#define X_MIN_PIN 9 +#define X_MAX_PIN -1 + +#define Y_STEP_PIN 3 +#define Y_DIR_PIN 6 +#define Y_ENABLE_PIN 8 +#define Y_MIN_PIN 10 +#define Y_MAX_PIN -1 + +#define Z_STEP_PIN 4 +#define Z_DIR_PIN 7 +#define Z_ENABLE_PIN 8 +#define Z_MIN_PIN 11 +#define Z_MAX_PIN -1 + +#define E0_STEP_PIN -1 +#define E0_DIR_PIN -1 +#define E0_ENABLE_PIN -1 +#define E0_MIN_PIN -1 + +#define E1_STEP_PIN -1 +#define E1_DIR_PIN -1 +#define E1_ENABLE_PIN -1 + +#define BYJ_PIN_0 -1 +#define BYJ_PIN_1 -1 +#define BYJ_PIN_2 -1 +#define BYJ_PIN_3 -1 + +#define SERVO_PIN 12 + +#define PUMP_PIN -1 +#define LASER_PIN -1 +#define LED_PIN -1 + +#define SDPOWER -1 +#define SDSS -1 + +#define FAN_PIN -1 + +#define PS_ON_PIN -1 +#define KILL_PIN -1 + +#define TEMP_0_PIN -1 // ANALOG NUMBERING +#define TEMP_1_PIN -1 // ANALOG NUMBERING + +#endif diff --git a/arduino_firmware/robotArm_v0.62/queue.h b/arduino_firmware/robotArm_v0.62/queue.h new file mode 100644 index 0000000..422395a --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/queue.h @@ -0,0 +1,79 @@ +#ifndef QUEUE_H_ +#define QUEUE_H_ + +template class Queue { +public: + Queue(int alen); + ~Queue(); + bool push(Element elem); + Element pop(); + bool isFull() const; + bool isEmpty() const; + int getFreeSpace() const; + int getMaxLength() const; + inline int getUsedSpace() const; +private: + Queue(Queue& q); //copy const. + Element* data; + int len; + int start; + int count; +}; + +template +Queue::Queue(int alen) { + data = new Element[alen]; + len = alen; + start = 0; + count = 0; +} + +template +Queue::~Queue() { + delete data; +} + +template +Queue::Queue(Queue& q) { + //nothing ever is allowed to do something here +} + +template +bool Queue::push(Element elem) { + data[(start + count++) % len] = elem; +} + +template +Element Queue::pop() { + count--; + int s = start; + start = (start + 1) % len; + return data[(s) % len]; +} + +template +bool Queue::isFull() const { + return count >= len; +} + +template +bool Queue::isEmpty() const { + return count <= 0; +} + +template +int Queue::getFreeSpace() const { + return len - count; +} + +template +int Queue::getMaxLength() const { + return len; +} + +template +int Queue::getUsedSpace() const { + return count; +} + +#endif diff --git a/arduino_firmware/robotArm_v0.62/robotArm_v0.62.ino b/arduino_firmware/robotArm_v0.62/robotArm_v0.62.ino new file mode 100644 index 0000000..df964fa --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/robotArm_v0.62.ino @@ -0,0 +1,271 @@ +//20SFFACTORY COMMUNITY ROBOT FIRMWARE v0.62 + +//V0.31 WITH G92, M114, LOGGER, LIMIT_CHECK FUNCTIONS +//V0.41 WITH DUAL SHANK LENGTH SUPPORT +//V0.51 WITH SERVO GRIPPER +//V0.61 WITH ARDUINO UNO OPTION +//V0.62 CHANGE POSITION FORMAT IN M114, +// ADD ENDSTOP WITH FAILURE CHECK +// CHANGE FLOW ON homeSequence() FOR GENERALITY + +#include "config.h" +#include "logger.h" + +#include "pinout.h" +#if USE_UNO + #include "pinout_uno.h" +#endif + +#include + +#include "robotGeometry.h" +#include "interpolation.h" +#include "fanControl.h" +#include "RampsStepper.h" +#include "queue.h" +#include "command.h" +#include "byj_gripper.h" +#include "servo_gripper.h" +#include "equipment.h" +#include "endstop.h" + +//STEPPER OBJECTS +RampsStepper stepperHigher(X_STEP_PIN, X_DIR_PIN, X_ENABLE_PIN, INVERSE_X_STEPPER); +RampsStepper stepperLower(Y_STEP_PIN, Y_DIR_PIN, Y_ENABLE_PIN, INVERSE_Y_STEPPER); +RampsStepper stepperRotate(Z_STEP_PIN, Z_DIR_PIN, Z_ENABLE_PIN, INVERSE_Z_STEPPER); + +//RAIL OBJECTS +RampsStepper stepperRail(E0_STEP_PIN, E0_DIR_PIN, E0_ENABLE_PIN, INVERSE_E0_STEPPER); +Endstop endstopE0(E0_MIN_PIN, E0_DIR_PIN, E0_STEP_PIN, E0_ENABLE_PIN, E0_MIN_INPUT, E0_HOME_STEPS, HOME_DWELL, E0_CHECK_DELAY); + +//ENDSTOP OBJECTS +Endstop endstopX(X_MIN_PIN, X_DIR_PIN, X_STEP_PIN, X_ENABLE_PIN, X_MIN_INPUT, X_HOME_STEPS, HOME_DWELL, X_CHECK_DELAY); +Endstop endstopY(Y_MIN_PIN, Y_DIR_PIN, Y_STEP_PIN, Y_ENABLE_PIN, Y_MIN_INPUT, Y_HOME_STEPS, HOME_DWELL, Y_CHECK_DELAY); +Endstop endstopZ(Z_MIN_PIN, Z_DIR_PIN, Z_STEP_PIN, Z_ENABLE_PIN, Z_MIN_INPUT, Z_HOME_STEPS, HOME_DWELL, Z_CHECK_DELAY); + +//EQUIPMENT OBJECTS +BYJ_Gripper byj_gripper(BYJ_PIN_0, BYJ_PIN_1, BYJ_PIN_2, BYJ_PIN_3, BYJ_GRIP_STEPS); +Servo_Gripper servo_gripper(SERVO_PIN, SERVO_GRIP_DEGREE, SERVO_UNGRIP_DEGREE); +Equipment laser(LASER_PIN); +Equipment pump(PUMP_PIN); +Equipment led(LED_PIN); +FanControl fan(FAN_PIN, FAN_DELAY); + +//EXECUTION & COMMAND OBJECTS +RobotGeometry geometry(END_EFFECTOR_OFFSET, LOW_SHANK_LENGTH, HIGH_SHANK_LENGTH); +Interpolation interpolator; +Queue queue(QUEUE_SIZE); +Command command; + +void setup() +{ + Serial.begin(BAUD); + stepperHigher.setPositionRad(PI / 2.0); // 90° + stepperLower.setPositionRad(0); // 0° + stepperRotate.setPositionRad(0); // 0° + stepperRail.setPosition(0); + if (HOME_ON_BOOT) { //HOME DURING SETUP() IF HOME_ON_BOOT ENABLED + homeSequence(); + Logger::logINFO("ROBOT ONLINE"); + } else { + setStepperEnable(false); //ROBOT ADJUSTABLE BY HAND AFTER TURNING ON + if (HOME_X_STEPPER && HOME_Y_STEPPER && !HOME_Z_STEPPER){ + Logger::logINFO("ROBOT ONLINE"); + Logger::logINFO("ROTATE ROBOT TO FACE FRONT CENTRE & SEND G28 TO CALIBRATE"); + } + if (HOME_X_STEPPER && HOME_Y_STEPPER && HOME_Z_STEPPER){ + Logger::logINFO("ROBOT ONLINE"); + Logger::logINFO("SEND G28 TO CALIBRATE"); + } + if (!HOME_X_STEPPER && !HOME_Y_STEPPER){ + Logger::logINFO("ROBOT ONLINE"); + Logger::logINFO("HOME ROBOT MANUALLY & SEND G28 TO CALIBRATE"); + } + } + interpolator.setInterpolation(INITIAL_X, INITIAL_Y, INITIAL_Z, INITIAL_E0, INITIAL_X, INITIAL_Y, INITIAL_Z, INITIAL_E0); +} + +void loop() { + interpolator.updateActualPosition(); + geometry.set(interpolator.getXPosmm(), interpolator.getYPosmm(), interpolator.getZPosmm()); + stepperRotate.stepToPositionRad(geometry.getRotRad()); + stepperLower.stepToPositionRad(geometry.getLowRad()); + stepperHigher.stepToPositionRad(geometry.getHighRad()); + if (RAIL){ + stepperRail.stepToPositionMM(interpolator.getEPosmm(), STEPS_PER_MM_RAIL); + } + stepperRotate.update(); + stepperLower.update(); + stepperHigher.update(); + if (RAIL){ + stepperRail.update(); + } + fan.update(); + + if (!queue.isFull()) { + if (command.handleGcode()) { + queue.push(command.getCmd()); + } + } + if ((!queue.isEmpty()) && interpolator.isFinished()) { + executeCommand(queue.pop()); + if (PRINT_REPLY) { + Serial.println(PRINT_REPLY_MSG); + } + } + if (millis() % 500 < 250) { + led.cmdOn(); + } + else { + led.cmdOff(); + } +} + +void executeCommand(Cmd cmd) { + + if (cmd.id == -1) { + printErr(); + return; + } + + if (cmd.id == 'G') { + switch (cmd.num) { + case 0: + case 1: + fan.enable(true); + Point posoffset; + posoffset = interpolator.getPosOffset(); + cmdMove(cmd, interpolator.getPosmm(), posoffset, command.isRelativeCoord); + interpolator.setInterpolation(cmd.valueX, cmd.valueY, cmd.valueZ, cmd.valueE, cmd.valueF); + Logger::logINFO("LINEAR MOVE: [X:" + String(cmd.valueX-posoffset.xmm) + " Y:" + String(cmd.valueY-posoffset.ymm) + " Z:" + String(cmd.valueZ-posoffset.zmm) + " E:" + String(cmd.valueE-posoffset.emm) + "]"); + break; + case 4: cmdDwell(cmd); break; + case 28: + if (USE_UNO){ + homeSequence_UNO(); + break; + } else { + homeSequence(); + break; + } + case 90: command.cmdToAbsolute(); break; // ABSOLUTE COORDINATE MODE + case 91: command.cmdToRelative(); break; // RELATIVE COORDINATE MODE + case 92: + interpolator.resetPosOffset(); + cmdMove(cmd, interpolator.getPosmm(), interpolator.getPosOffset(), false); + interpolator.setPosOffset(cmd.valueX, cmd.valueY, cmd.valueZ, cmd.valueE); + break; + default: printErr(); + } + } + else if (cmd.id == 'M') { + switch (cmd.num) { + case 1: pump.cmdOn(); break; + case 2: pump.cmdOff(); break; + case 3: + if (GRIPPER == 0){ + byj_gripper.cmdOn(); break; + } else if (GRIPPER == 1){ + servo_gripper.cmdOn(); break; + } + case 5: + if (GRIPPER == 0){ + byj_gripper.cmdOff(); break; + } else if (GRIPPER == 1){ + servo_gripper.cmdOff(); break; + } + case 6: laser.cmdOn(); break; + case 7: laser.cmdOff(); break; + case 17: setStepperEnable(true); break; + case 18: setStepperEnable(false); break; + case 106: fan.enable(true); break; + case 107: fan.enable(false); break; + case 114: command.cmdGetPosition(interpolator.getPosmm(), interpolator.getPosOffset(), stepperHigher.getPosition(), stepperLower.getPosition(), stepperRotate.getPosition()); break;// Return the current positions of all axis + case 119: + String endstopMsg = "ENDSTOP: [X:"; + endstopMsg += String(endstopX.state()); + endstopMsg += " Y:"; + endstopMsg += String(endstopY.state()); + endstopMsg += " Z:"; + endstopMsg += String(endstopZ.state()); + endstopMsg += "]"; + //ORIGINAL LOG STRING UNDESIRABLE FOR UNO PROCESSING + //Logger::logINFO("ENDSTOP STATE: [UPPER_SHANK(X):"+String(endstopX.state())+" LOWER_SHANK(Y):"+String(endstopY.state())+" ROTATE_GEAR(Z):"+String(endstopZ.state())+"]"); + Logger::logINFO(endstopMsg); + break; + default: printErr(); + } + } + else { + printErr(); + } +} + +void setStepperEnable(bool enable){ + stepperRotate.enable(enable); + stepperLower.enable(enable); + stepperHigher.enable(enable); + if(RAIL){ + stepperRail.enable(enable); + } + fan.enable(enable); +} + +void homeSequence(){ + setStepperEnable(false); + fan.enable(true); + if (!HOME_Y_STEPPER || !HOME_X_STEPPER){ + setStepperEnable(true); + endstopY.homeOffset(!INVERSE_Y_STEPPER); + endstopX.homeOffset(!INVERSE_X_STEPPER); + } else { + if (HOME_Y_STEPPER){ + endstopY.home(!INVERSE_Y_STEPPER); //INDICATE STEPPER HOMING DIRECTION + } + if (HOME_X_STEPPER){ + endstopX.home(!INVERSE_X_STEPPER); //INDICATE STEPPER HOMING DIRECTION + } + } + if (HOME_Z_STEPPER){ + endstopZ.home(INVERSE_Z_STEPPER); //INDICATE STEPPER HOMING DIRECTION + } + + if (RAIL){ + if (HOME_E0_STEPPER){ + endstopE0.home(!INVERSE_E0_STEPPER); // + } + } + if((HOME_Y_STEPPER and endstopY.checkDelay()) + or (HOME_X_STEPPER and endstopX.checkDelay()) + or (HOME_Z_STEPPER and endstopZ.checkDelay()) + or (RAIL and HOME_E0_STEPPER and endstopE0.checkDelay()) ){ + while(!queue.isEmpty()) { + queue.pop(); + } + Logger::logERROR("ROBOT FAILURE"); + }else{ + interpolator.setInterpolation(INITIAL_X, INITIAL_Y, INITIAL_Z, INITIAL_E0, INITIAL_X, INITIAL_Y, INITIAL_Z, INITIAL_E0); + Logger::logINFO("HOMING COMPLETE"); + } +} + +//DUE TO UNO CNC SHIELD LIMIT, 1 EN PIN SERVES 3 MOTORS, HENCE DIFFERENT HOMESEQUENCE IS REQUIRED +void homeSequence_UNO(){ + if (HOME_Y_STEPPER && HOME_X_STEPPER){ + while (!endstopY.state() || !endstopX.state()){ + endstopY.oneStepToEndstop(!INVERSE_Y_STEPPER); + endstopX.oneStepToEndstop(!INVERSE_X_STEPPER); + } + endstopY.homeOffset(!INVERSE_Y_STEPPER); + endstopX.homeOffset(!INVERSE_X_STEPPER); + } else { + setStepperEnable(true); + endstopY.homeOffset(!INVERSE_Y_STEPPER); + endstopX.homeOffset(!INVERSE_X_STEPPER); + } + if (HOME_Z_STEPPER){ + endstopZ.home(INVERSE_Z_STEPPER); //INDICATE STEPPER HOMING DIRECDTION + } + interpolator.setInterpolation(INITIAL_X, INITIAL_Y, INITIAL_Z, INITIAL_E0, INITIAL_X, INITIAL_Y, INITIAL_Z, INITIAL_E0); + Logger::logINFO("HOMING COMPLETE"); +} diff --git a/arduino_firmware/robotArm_v0.62/robotGeometry.cpp b/arduino_firmware/robotArm_v0.62/robotGeometry.cpp new file mode 100644 index 0000000..74dafa3 --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/robotGeometry.cpp @@ -0,0 +1,61 @@ +#include "robotGeometry.h" + +#include +#include + +RobotGeometry::RobotGeometry(float a_ee_offset, float a_low_shank_length, float a_high_shank_length) { + ee_offset = a_ee_offset; + low_shank_length = a_low_shank_length; + high_shank_length = a_high_shank_length; +} + +void RobotGeometry::set(float axmm, float aymm, float azmm) { + xmm = axmm; + ymm = aymm; + zmm = azmm; + calculateGrad(); +} + +float RobotGeometry::getXmm() const { + return xmm; +} + +float RobotGeometry::getYmm() const { + return ymm; +} + +float RobotGeometry::getZmm() const { + return zmm; +} + +float RobotGeometry::getRotRad() const { + return rot; +} + +float RobotGeometry::getLowRad() const { + return low; +} + +float RobotGeometry::getHighRad() const { + return high; +} + +void RobotGeometry::calculateGrad() { + 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 = 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 = acos(zmm / rside) - acos((low_2 - high_2 + rside_2) / (2 * low_shank_length * rside)); + } else { + low = PI - asin(rrot / rside) - acos((low_2 - high_2 + rside_2) / (2 * low_shank_length * rside)); + } + high = high + low; +} diff --git a/arduino_firmware/robotArm_v0.62/robotGeometry.h b/arduino_firmware/robotArm_v0.62/robotGeometry.h new file mode 100644 index 0000000..2b7d572 --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/robotGeometry.h @@ -0,0 +1,27 @@ +#ifndef ROBOTGEOMETRY_H_ +#define ROBOTGEOMETRY_H_ + +class RobotGeometry { +public: + 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; + float getZmm() const; + float getRotRad() const; + float getLowRad() const; + float getHighRad() const; +private: + void calculateGrad(); + float ee_offset; + float low_shank_length; + float high_shank_length; + float xmm; + float ymm; + float zmm; + float rot; + float low; + float high; +}; + +#endif diff --git a/arduino_firmware/robotArm_v0.62/servo_gripper.cpp b/arduino_firmware/robotArm_v0.62/servo_gripper.cpp new file mode 100644 index 0000000..859dcf2 --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/servo_gripper.cpp @@ -0,0 +1,25 @@ +#include "servo_gripper.h" +#include +#include + + +Servo_Gripper::Servo_Gripper(int pin, float grip_degree, float ungrip_degree){ + servo_pin = pin; + servo_grip_deg = grip_degree; + servo_ungrip_deg = ungrip_degree; + Servo servo_motor; +} + +void Servo_Gripper::cmdOn(){ + servo_motor.attach(servo_pin); + servo_motor.write(servo_grip_deg); + delay(300); + servo_motor.detach(); +} + +void Servo_Gripper::cmdOff(){ + servo_motor.attach(servo_pin); + servo_motor.write(servo_ungrip_deg); + delay(300); + servo_motor.detach(); +} diff --git a/arduino_firmware/robotArm_v0.62/servo_gripper.h b/arduino_firmware/robotArm_v0.62/servo_gripper.h new file mode 100644 index 0000000..fdf7933 --- /dev/null +++ b/arduino_firmware/robotArm_v0.62/servo_gripper.h @@ -0,0 +1,17 @@ +#ifndef SERVO_GRIPPER_H_ +#define SERVO_GRIPPER_H_ +#include + +class Servo_Gripper{ +public: + Servo_Gripper(int pin, float grip_degree, float ungrip_degree); + void cmdOn(); + void cmdOff(); +private: + Servo servo_motor; + int servo_pin; + float servo_grip_deg; + float servo_ungrip_deg; +}; + +#endif