#include #include #include //debugging LED const int ledPin = 13; // Define arm Constants const float a = 10; // lower joint length (cm) const float b = 8.5; // upper joint length (cm) // Correction factors to align servo values with their respective axis const float S_1_CorrectionFactor = 4; // Align arm "a" with the horizontal when at 0 degrees const float S_2_CorrectionFactor = -3; // Align arm "b" with arm "a" when at 0 degrees // Correction factor to shift origin out to edge of the mount const float X_CorrectionFactor = -1.5; // X direction correction factor (cm) const float Y_CorrectionFactor = 7; // Y direction correction factor (cm) // Angle Variables float A; //Angle oppposite side a (between b and c) float B; //Angle oppposite side b float C; //Angle oppposite side c float theta; //Angle formed between line from origin to (x,y) and the horizontal float Xheta; //Angle formed between line from origin to (x,y) and the vertical // Distance variables float x; // x position (cm) float y; // y position (cm) float c; // Hypotenuse legngth in cm const float pi = M_PI; //Store pi in a less annoying format //Circle Setup const float Radius = 2.5; // Radius of circle in centimeters const int CircleCenterX = 0; // X coordinate of the center of the circle const int CircleCenterY = 0; // Y coordinate of the center of the circle //===================================================================== //===========STEPPERS================================================= //===================================================================== #define HALFSTEP 8 // Определение пинов для управления двигателем 1 #define motor1Pin1 8 // IN1 на 1-м драйвере ULN2003 #define motor1Pin2 9 // IN2 на 1-м драйвере ULN2003 #define motor1Pin3 10 // IN3 на 1-м драйвере ULN2003 #define motor1Pin4 11 // IN4 на 1-м драйвере ULN2003 #define button 7 AccelStepper stepper1(HALFSTEP, motor1Pin1, motor1Pin3, motor1Pin2, motor1Pin4); const int homeButton1 = 7; byte hBval; // Определение пинов для управления двигателем 2 #define motor2Pin1 2 // IN1 на 2-м драйвере ULN2003 #define motor2Pin2 3 // IN2 на 2-м драйвере ULN2003 #define motor2Pin3 4 // IN3 на 2-м драйвере ULN2003 #define motor2Pin4 5 // IN4 на 2-м драйвере ULN2003 #define button 6 AccelStepper stepper2(HALFSTEP, motor2Pin1, motor2Pin3, motor2Pin2, motor2Pin4); const int homeButton2 = 6; byte hBva2; //===================================================================== //==============SERVO================================================= //===================================================================== Servo Servo_p; // pen joint const float serv_ang1 = 90; // angle for up pen position const float serv_ang2 = 135; // angle for down pen position #include //===================================================================== //==============SETUP================================================= //===================================================================== void setup(){ //steppers setup stepper1.setMaxSpeed(1000); //nice and slow for testing stepper1.setAcceleration(600); stepper1.setSpeed(800); pinMode(homeButton1, INPUT_PULLUP); stepper2.setMaxSpeed(1000); //nice and slow for testing stepper2.setAcceleration(600); stepper2.setSpeed(900); pinMode(homeButton2, INPUT_PULLUP); //servo setup Servo_p.attach(12); // pen joint pinMode(ledPin, OUTPUT); // -For debugging Serial.begin(9600); // -For debugging Serial.println(0); servo_pHome(); //runs routine to home servo_p stepper2Home(); //runs routine to home motor1 stepper1Home(); //runs routine to home motor2 } //===================================================================== //==============LOOP=================================================== //===================================================================== //parabola //x = sq(t/2)/10 ; //y = t/2 ; // Equation of a circle in parametric form //x = Radius * cos(t *(pi/180)) + CircleCenterX; //y = Radius * sin(t *(pi/180)) + CircleCenterY; void loop() { for (float t = 0; t<=9; t++) // Get t value { x = 0 ; y = t ; DoAlltheStuff(); } for (float t = 0; t<=9; t++) // Get t value { x = -t ; y = 9-t ; DoAlltheStuff(); } for (float t = -9; t<=9; t++) // Get t value { x = t ; y = 0 ; DoAlltheStuff(); } for (float t = 0; t<=9; t++) // Get t value { x = 9-t ; y = t ; DoAlltheStuff(); } } //===================================================================== //==============HOMING-RUTINE========================================== //===================================================================== void stepper1Home() //this routine should run the motor { hBval = digitalRead(homeButton1); while (hBval == LOW) { //backwards slowly till it hits the switch and stops stepper1.moveTo(-6000); stepper1.run(); hBval = digitalRead(homeButton1); } delay(1000); stepper1.setCurrentPosition(0); //should set motor position to zero and go back to main routine delay(1000); } void stepper2Home() //this routine should run the motor { hBva2 = digitalRead(homeButton2); while (hBva2 == LOW) { //backwards slowly till it hits the switch and stops stepper2.moveTo(4000); stepper2.run(); hBva2 = digitalRead(homeButton2); } delay(1000); stepper2.setCurrentPosition(0); //should set motor position to zero and go back to main routine delay(1000); } void servo_pHome(){ digitalWrite(ledPin, HIGH); delay(500); digitalWrite(ledPin, LOW); delay(500); digitalWrite(ledPin, HIGH); delay(500); digitalWrite(ledPin, LOW); delay(500); Servo_p.write(serv_ang1); // risen pen delay(1000); } //======================CALCULATING============================================== //gets x and y measured from the bottom of the base. function corrects for offset void FixCoordinates(float x_input, float y_input) { x = x_input + X_CorrectionFactor; y = y_input + Y_CorrectionFactor; } //calculates necessary servo angles to move arm to desired points void CalculateServoAngles() { if (x >= 0) { c = sqrt( sq(x) + sq(y) ); // pythagorean theorem B = (acos( (sq(b) - sq(a) - sq(c))/(-2*a*c) )) * (180/pi); // Law of cosines: Angle opposite upper arm section C = 180-(acos( (sq(c) - sq(b) - sq(a))/(-2*a*b) )) * (180/pi); // Law of cosines: Angle opposite hypotenuse theta = (asin( y / c )) * (180/pi); // Solve for theta to correct for lower joint's impact on upper joint's angle ServoS_1_Angle = ( B + theta + S_1_CorrectionFactor ) * 11,38; // Find necessary angle. Add Correction. Multiply by gear ratio ServoS_2_Angle = ( C + S_2_CorrectionFactor ) * 11,38; // Find neceesary angle. Add Correction. Multiply by gear ratio } if (x < 0) { c = sqrt( sq(x) + sq(y) ); // pythagorean theorem B = (acos( (sq(b) - sq(a) - sq(c))/(-2*a*c) )) * (180/pi); // Law of cosines: Angle opposite upper arm section C = 180-(acos( (sq(c) - sq(b) - sq(a))/(-2*a*b) )) * (180/pi); // Law of cosines: Angle opposite hypotenuse theta = (asin( y / c )) * (180/pi); // Solve for theta to correct for lower joint's impact on upper joint's angle Xheta = 90 - theta; // find the oopsit of theta ServoS_1_Angle = ( B + theta + 2*Xheta + S_1_CorrectionFactor ) * 11,38; // Find necessary angle. Add Correction for negative X. Multiply by gear ratio ServoS_2_Angle = ( C + S_2_CorrectionFactor ) * 11,38; // Find neceesary angle. Add Correction. Multiply by gear ratio Serial.println(ServoS_2_Angle); } } //updates the motors void MoveArm() { Serial.println(22); stepper1.moveTo(ServoS_1_Angle); stepper2.moveTo(-ServoS_2_Angle); stepper1.runToPosition(); stepper2.runToPosition(); } //lowers pen on paper void DoAlltheStuff() { FixCoordinates(x,y); // Enter coordinates of point. CalculateServoAngles(); // Calculate angle of servos MoveArm(); // Move arm to new position //delay(200); // Draw_pen(); //delay(500); } void Draw_pen() { digitalWrite(ledPin, HIGH); Servo_p.write(serv_ang2); // lower pen delay(700); digitalWrite(ledPin, LOW); Servo_p.write(serv_ang1); // risen pen }