#include "Arduino.h" #include "4AxisRobotArm.h" #include //Stepper Motor Library #include //Stepper Motor Library double doubleMap(double x, double in_min, double in_max, double out_min, double out_max){ return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } ///Defining the constructers of the stepper Struct //Constructor to Handle auto calculation of DegPerStep stepper::stepper(byte stepPin, byte dirPin, byte enPin, byte endSwitchPin, double LowRange, double HighRange, long MotorStepsRange){ this->stepPin = stepPin; this->dirPin = dirPin; this->enPin = enPin; this->endSwitchPin = endSwitchPin; this->LowRange = LowRange; this->HighRange = HighRange; this->MotorStepsRange = MotorStepsRange; DegPerStep = fabs(HighRange - LowRange) / MotorStepsRange; } //Constructor to Handle manual input of DegPerStep stepper::stepper(byte stepPin, byte dirPin, byte enPin, byte endSwitchPin, double LowRange, double HighRange, double DegPerStep ,long MotorStepsRange){ this->stepPin = stepPin; this->dirPin = dirPin; this->enPin = enPin; this->endSwitchPin = endSwitchPin; this->LowRange = LowRange; this->HighRange = HighRange; this->MotorStepsRange = MotorStepsRange; this->DegPerStep = DegPerStep; } //Robot Arm Class section //Defining the constructor for the Robot Arm Class RobotArm::RobotArm(stepper stepper1, stepper stepper2, stepper stepper3, stepper stepper4, int ArmLength1,int ArmLength2, int ArmLength3, bool Safety) : stepper1(stepper1), stepper2(stepper2), stepper3(stepper3), stepper4(stepper4), ArmLength1(ArmLength1), ArmLength2(ArmLength2), ArmLength3(ArmLength3), Safety(Safety), Motor1(AccelStepper::DRIVER, stepper1.stepPin, stepper1.dirPin), Motor2(AccelStepper::DRIVER, stepper2.stepPin, stepper2.dirPin), Motor3(AccelStepper::DRIVER, stepper3.stepPin, stepper3.dirPin), Motor4(AccelStepper::DRIVER, stepper4.stepPin, stepper4.dirPin) { //Setting EndSwitch PinModes pinMode(stepper1.endSwitchPin, INPUT_PULLUP); pinMode(stepper2.endSwitchPin, INPUT_PULLUP); pinMode(stepper3.endSwitchPin, INPUT_PULLUP); pinMode(stepper4.endSwitchPin, INPUT); //Setting Enable PinModes pinMode(stepper1.enPin, OUTPUT); pinMode(stepper2.enPin, OUTPUT); pinMode(stepper3.enPin, OUTPUT); pinMode(stepper4.enPin, OUTPUT); digitalWrite(stepper1.enPin, HIGH); digitalWrite(stepper2.enPin, HIGH); digitalWrite(stepper3.enPin, HIGH); digitalWrite(stepper4.enPin, HIGH); //Adding Motors to MultiStepper Object steppers.addStepper(Motor1); steppers.addStepper(Motor2); steppers.addStepper(Motor3); steppers.addStepper(Motor4); //Setting default MaxSpeed long defaultMaxSpeed = 1000; //1000 steps per second Motor1.setMaxSpeed(defaultMaxSpeed); Motor2.setMaxSpeed(defaultMaxSpeed); Motor3.setMaxSpeed(defaultMaxSpeed); Motor4.setMaxSpeed(defaultMaxSpeed); //Setting appropriate pin inversions Motor4.setPinsInverted(true,false,false); Motor3.setPinsInverted(false,false,false); Motor2.setPinsInverted(false,false,false); Motor1.setPinsInverted(true,false,false); } /// @brief Handles the position of joint Angles (Moves joints) /// @param ZOrio Angular Base Orientation in Z Axis /// @param Joint1Angle Angle of the first joint. Note. Joints are considered from bottom to top /// @param Joint2Angle Angle of the second joint. Note. Joints are considered from bottom to top /// @param Joint3Angle Angle of the third joint. Note. Joints are considered from bottom to top /// @note For some Joints the direction of motion is inverted to correlate with the stepper placement void RobotArm::MoveRobotArmJoints(double ZOrio, double Joint1Angle, double Joint2Angle, double Joint3Angle){ // Finding the number of steps Base Motor has to move // Convert ZOrio into local steps and constrain if possible ZOrio = constrain(ZOrio, -(stepper1.HighRange - stepper1.LowRange) / 2, (stepper1.HighRange - stepper1.LowRange) / 2); //Constraining to angular Range double ZOrioChange = doubleMap(ZOrio, -(stepper1.HighRange - stepper1.LowRange) / 2, (stepper1.HighRange - stepper1.LowRange) / 2, 0.0, (stepper1.HighRange - stepper1.LowRange)); //Mapping referencial reference to 0 long BaseSteps = ZOrioChange / stepper1.DegPerStep; // Converting Angular Change to steps // Finding the number of steps the first Joint has to move // Constraining to angular range Joint1Angle = constrain(Joint1Angle, stepper2.LowRange, stepper2.HighRange); // Map referencial angle to Change in angle and inverting it double Joint1AngleChange = doubleMap(Joint1Angle, stepper2.LowRange, stepper2.HighRange, (stepper2.HighRange - stepper2.LowRange), 0.0 ); // Converting Joint1AngleChange to steps long Joint1Steps = Joint1AngleChange / stepper2.DegPerStep; // Constrain steps if MaxStepRange is available if(stepper2.MotorStepsRange > 0){ Joint1Steps = constrain(Joint1Steps, 0, stepper2.MotorStepsRange); } // Finding the number of steps the second Joint has to move // Constraining to angular range Joint2Angle = constrain(Joint2Angle, stepper3.LowRange, stepper3.HighRange); // Map referencial angle to Change in angle double Joint2AngleChange = doubleMap(Joint2Angle, stepper3.LowRange, stepper3.HighRange, 0.0, (stepper3.HighRange - stepper3.LowRange)); // Converting Joint2AngleChange to steps long Joint2Steps = Joint2AngleChange / stepper3.DegPerStep; // Constrain steps if MaxStepRange is available if(stepper3.MotorStepsRange > 0){ Joint2Steps = constrain(Joint2Steps, 0, stepper3.MotorStepsRange); } // Finding the number of steps the third Joint has to move // Constraining to angular range Joint3Angle = constrain(Joint3Angle, stepper4.LowRange, stepper4.HighRange); // Map referencial angle to Change in angle and inverting it double Joint3AngleChange = doubleMap(Joint3Angle, stepper4.LowRange, stepper4.HighRange, (stepper4.HighRange - stepper4.LowRange), 0.0); // Converting Joint3AngleChange to steps long Joint3Steps = Joint3AngleChange / stepper4.DegPerStep; // Constrain steps if MaxStepRange is available if(stepper4.MotorStepsRange > 0){ Joint3Steps = constrain(Joint3Steps, 0, stepper4.MotorStepsRange); } // Create Array of JointSteps in exact numbers long position[] = { BaseSteps, Joint1Steps, Joint2Steps, Joint3Steps }; //Serial.println(position[0]); //Serial.println(position[1]); //Serial.println(position[2]); //Serial.println(position[3]); //Set StepperMotor Speeds float speed = 3000; Motor1.setMaxSpeed(speed * 3); Motor2.setMaxSpeed(speed); Motor3.setMaxSpeed(speed); Motor4.setMaxSpeed(speed); //Set Steppers Position // Serial.println(position[0]); // Serial.println(position[1]); //Serial.println(position[2]); //Serial.println(position[3]); steppers.moveTo(position); } void RobotArm::MoveRobotArmToPosition(double x, double y, double z, double Orio){ RobotAngles Angles = InverseKinematics3D(x, y, z, Orio); // Calculates Robot Arm Angles MoveRobotArmJoints(Angles.theta0, Angles.theta1, Angles.theta2, Angles.theta3); //Moves Robot Arm Angles to that position } bool RobotArm::IsRobotMoving(){ /* Serial.println("Motor1: "); Serial.println(Motor1.distanceToGo()); Serial.println("Motor2: "); Serial.println(Motor2.distanceToGo()); Serial.println("Motor3: "); Serial.println(Motor3.distanceToGo()); Serial.println("Motor4: "); Serial.println(Motor4.distanceToGo());*/ if( Motor1.distanceToGo() == 0 && Motor2.distanceToGo() == 0 && Motor3.distanceToGo() == 0 && Motor4.distanceToGo() == 0){ return false; // returns true if any of the motors haven't reached their final position } return true; // returns false if all motors have stopped moving } ///@brief Converts X Y Z, and End Effector Angle into Robot Arm Angles RobotArm::RobotAngles RobotArm::InverseKinematics3D(float x, float y, float z, float Orio){ //Converts Our angle into radians double OrioRad = radians(Orio); /*Since this is 3D by virtue of Base Arm rotation We can convert this into 2D plane inverse kinematics, after finding the base roation */ double theta0 = atan2(y,x); // Calculating base angle //Shifting to 2D Plane Kinematics double l = sqrt(square(x) + square(y)); //Calculating the X Coordinate in the New plane // Z Axis becomes the new Y Axis double Xw = l - (ArmLength3 * cos(OrioRad)); // Calulating the X Coordinate of the 3rd Joint double Yw = z - (ArmLength3 * sin(OrioRad)); // Calculating the Y Coordinate of the 3rd Joint //Finding Joint2 Angle With some trig double r = sqrt( square(Xw) + square(Yw)); double theta2 = acos(constrain((square(ArmLength1) + square(ArmLength2) - square(r))/(2.0 * ArmLength1 * ArmLength2), -1, 1)); //Finding Joint1 Angle With some trig double thetaA = atan2(Yw,Xw); double thetaB = acos(constrain((square(r) + square(ArmLength1) - square(ArmLength2))/(2.0 * ArmLength1 * r), -1, +1)); double theta1 = thetaA + thetaB; // Adding both Angles to compute Angle of th first joint //Finding third joint Angle using end effector Angle double theta3 = OrioRad - theta1 - theta2; //Converting Radians to Degrees double theta0deg = degrees(theta0); double theta1deg = degrees(theta1); double theta2deg = degrees(theta2); double theta3deg = degrees(theta3); //Making sure theta3 is positive; if(theta3deg < 0){ theta3deg += 360; //Adding 360 degrees to negative theta3 angle to turn positive } //Returning theta values //RobotAngles Angles; Angles.theta0 = theta0deg; Angles.theta1 = theta1deg; Angles.theta2 = theta2deg; Angles.theta3 = theta3deg; return Angles; } void RobotArm::Home(bool HomeMot1, bool HomeMot2, bool HomeMot3, bool HomeMot4){ // long HighMaxSpeed = 10000; // long LowMaxSpeed = 10000; //Activating stepper motors digitalWrite(stepper1.enPin, LOW); digitalWrite(stepper2.enPin, LOW); digitalWrite(stepper3.enPin, LOW); digitalWrite(stepper4.enPin, LOW); //Homing Motor2 if( HomeMot2 == true ){ while(digitalRead(stepper2.endSwitchPin) == 1){ Motor2.setMaxSpeed(10000); Motor2.setSpeed(8000); Motor2.moveTo(-100000); Motor2.runSpeedToPosition(); } Motor2.setCurrentPosition(-600); Motor2.setMaxSpeed(1000); Motor2.setSpeed(1000); long positions[4] = {0,0,0,0}; steppers.moveTo(positions); steppers.runSpeedToPosition(); while(digitalRead(stepper2.endSwitchPin) == 1){ Motor2.setSpeed(10000); Motor2.setSpeed(1000); Motor2.moveTo(-100000); Motor2.runSpeedToPosition(); } Motor2.setCurrentPosition(-300); Motor2.setMaxSpeed(1000); Motor2.setSpeed(1000); // positions[4] = {0,0,0,0}; steppers.moveTo(positions); steppers.runSpeedToPosition(); } if( HomeMot3 == true ){ while(digitalRead(stepper3.endSwitchPin) == 1){ Motor3.setMaxSpeed(10000); Motor3.setSpeed(6000); Motor3.moveTo(-100000); Motor3.runSpeedToPosition(); } Motor3.setCurrentPosition(-400); Motor3.setMaxSpeed(1000); Motor3.setSpeed(1000); long positions[4] = {0,0,0,0}; steppers.moveTo(positions); steppers.runSpeedToPosition(); while(digitalRead(stepper3.endSwitchPin) == 1){ Motor3.setSpeed(500); Motor3.moveTo(-100000); Motor3.runSpeedToPosition(); } Motor3.setCurrentPosition(-200); Motor3.setMaxSpeed(1000); Motor3.setSpeed(1000); // positions[4] = {0,0,0,0}; steppers.moveTo(positions); steppers.runSpeedToPosition(); } if( HomeMot4 == true ){ while(digitalRead(stepper4.endSwitchPin) == 1){ Motor4.setMaxSpeed(10000); Motor4.setSpeed(1500); Motor4.moveTo(-100000); Motor4.runSpeedToPosition(); } Motor4.setCurrentPosition(-200); Motor4.setMaxSpeed(1000); Motor4.setSpeed(1000); long positions[4] = {0,0,0,0}; steppers.moveTo(positions); steppers.runSpeedToPosition(); while(digitalRead(stepper4.endSwitchPin) == 1){ Motor4.setSpeed(100); Motor4.moveTo(-100000); Motor4.runSpeedToPosition(); } Motor4.setCurrentPosition(-100); Motor4.setMaxSpeed(1000); Motor4.setSpeed(1000); // poitions[4] = {0,0,0,0}; steppers.moveTo(positions); steppers.runSpeedToPosition(); } if( HomeMot1 == true ){ while(digitalRead(stepper1.endSwitchPin) == 1){ Motor1.setMaxSpeed(10000); Motor1.setSpeed(8000); Motor1.moveTo(-1000000); Motor1.runSpeedToPosition(); //Serial.println("I am homing"); } Motor1.setCurrentPosition(-200); Motor1.setMaxSpeed(1000); Motor1.setSpeed(1000); long positions[4] = {0,0,0,0}; steppers.moveTo(positions); steppers.runSpeedToPosition(); while(digitalRead(stepper1.endSwitchPin) == 1){ Motor1.setMaxSpeed(10000); Motor1.setSpeed(500); Motor1.moveTo(-1000000); Motor1.runSpeedToPosition(); //Serial.println("I am homing"); } Motor1.setCurrentPosition(-200); Motor1.setMaxSpeed(1000); Motor1.setSpeed(1000); //long positions[4] = {0,0,0,0}; steppers.moveTo(positions); steppers.runSpeedToPosition(); } } /// @brief This function deactivates the motor when it's respective endswitch is hit, and rehomes. void RobotArm::SafetyStop(){ long SwitchList[4] = {digitalRead(stepper1.endSwitchPin),digitalRead(stepper2.endSwitchPin),digitalRead(stepper3.endSwitchPin),digitalRead(stepper4.endSwitchPin)}; switch (SwitchList[0]){ case 1: digitalWrite(stepper1.enPin,LOW); break; case 0: digitalWrite(stepper1.enPin,HIGH); Serial.println("Endswitch 1 Activated"); Home(true, true , true , true); break; } switch (SwitchList[1]){ case 1: digitalWrite(stepper2.enPin,LOW); break; case 0: digitalWrite(stepper2.enPin,HIGH); Serial.println("Endswitch 2 Activated"); Home(true, true , true , false); break; } switch (SwitchList[2]){ case 1: digitalWrite(stepper3.enPin,LOW); break; case 0: digitalWrite(stepper3.enPin,HIGH); Serial.println("Endswitch 3 Activated"); Home(true, true , true , false); break; } switch (SwitchList[3]){ case 1: digitalWrite(stepper4.enPin,LOW); break; case 0: digitalWrite(stepper4.enPin,HIGH); Serial.println("Endswitch 4 Activated"); Home(true, true , true , false); break; } } /// @brief Function Should be ran as often as possible for predictable RobotMotion void RobotArm::RobotArmRun(){ // Stop Run function if pause is On_going if((millis() - time_pause_since_call) <= pause_time){ return; } //If safety is enabled to run the safety protection if(Safety == true){ SafetyStop(); } steppers.run(); //run Stepper Motors to position if new step is due. } double RobotArm::getMoveDuration(){ double time1 = abs(Motor1.targetPosition() - Motor1.currentPosition()) / Motor1.maxSpeed(); double time2 = abs(Motor2.targetPosition() - Motor2.currentPosition()) / Motor2.maxSpeed(); double time3 = abs(Motor3.targetPosition() - Motor3.currentPosition()) / Motor3.maxSpeed(); double time4 = abs(Motor4.targetPosition() - Motor4.currentPosition()) / Motor4.maxSpeed(); double maxTime = max(time1, max(time2, max(time3, time4))); //Serial.print("Time taken for robot to move: "); // Serial.println(maxTime, 3); return maxTime * 1.01; } /// @brief This function serves to pause the robot arm /// This function works by telling the robot arm run function /// that a pause was requested at set time /// and requested for set length void RobotArm::pause(unsigned long time){ time_pause_since_call = millis(); pause_time = time; }