/*
Robot arm test script 2.0 Dec12 alanesq@disroot.org
Uses 2 stepper motors to move a simple 2 dimensional robotic arm
the arm is positioned by giving an x,y coordinate and via the gift of inverse kinematics
this is translated to the required joint angles.
*/
#include <Servo.h> // servo library
// define robot arm mengths
#define humerus 134 // Humerus length (mm)
#define ulna 134 // Ulna length (mm)
//stepper motors
int motorSPin1 = 4; // Blue - 28BYJ48 pin 1
int motorSPin2 = 5; // Pink - 28BYJ48 pin 2
int motorSPin3 = 6; // Yellow - 28BYJ48 pin 3
int motorSPin4 = 7; // Orange - 28BYJ48 pin 4
int motorEPin1 = 8; // Blue - 28BYJ48 pin 1
int motorEPin2 = 9; // Pink - 28BYJ48 pin 2
int motorEPin3 = 10; // Yellow - 28BYJ48 pin 3
int motorEPin4 = 11; // Orange - 28BYJ48 pin 4
int motorSpeed = 1600; //variable to set stepper speed
int countsperrev = 4000; // number of steps per full revolution
int lookup[8] = {B01000, B01100, B00100, B00110, B00010, B00011, B00001, B01001}; // stepper coil sequence
int count = 0; // temp counter
// define global variables
int shoulderPos = 1000; // current position of shoulder (in stepper motor steps)
int elbowPos = 1000; // current position of elbow (in stepper motor steps)
int shoulderStep = 0; // current stepper motor sequence step (0-7)
int elbowStep = 0; // current stepper motor sequence step (0-7)
// pre calculations
float hum_sq = humerus * humerus; // humurus length squared
float uln_sq = ulna * ulna; // ulna length squared
// ***************************************************************************
void setup()
{
//start serial connection
Serial.begin(9600);
Serial.println("Robot arm demo sketch v2.0");
//declare the stepper motor pins as outputs
pinMode(motorSPin1, OUTPUT);
pinMode(motorSPin2, OUTPUT);
pinMode(motorSPin3, OUTPUT);
pinMode(motorSPin4, OUTPUT);
pinMode(motorEPin1, OUTPUT);
pinMode(motorEPin2, OUTPUT);
pinMode(motorEPin3, OUTPUT);
pinMode(motorEPin4, OUTPUT);
resetArm(); // reset arm to it's start position
}
// ***************************************************************************
void loop()
{
// if serial data waiting then read input and move arm
if(Serial.available()) moveArmFromSerial();
delay (100);
}
//// move arm along a straight line - up down
// while(1) {
// for ( int i=30; i < 210; i = i + 20) {
// moveArm ( 140,i );
// delay(300);
// }
// for ( int i=210; i > 30; i = i - 20) {
// moveArm ( 140,i );
// delay(300);
// }
// }
// // move arm along a straight line - left right
// while(1) {
// for ( int i=50; i < 200; i = i + 10) {
// moveArm ( i,140 );
// delay(300);
// }
// for ( int i=220; i > 50; i = i - 10) {
// moveArm ( i,140 );
// delay(300);
// }
// }
// // move around in a circle
// float h = 130.0; // x coordinate of circle center
// float k = 130.0; // y coordinate of circle center
// float r = 50.0; // radius of circle
// float cstep = 6.283 / 60; // steps (2 pi divided by steps)
// while(1) {
// for ( float theta = 0; theta < 6.283; theta = theta + cstep) {
// float x = h + r*cos(theta);
// float y = k + r*sin(theta);
// moveArm( x,y);
// }
// }
// ***************************************************************************
// procedures
// ***************************************************************************
// Move the arm to specified x,y position
void moveArm ( float x, float y) {
// calculate required angles using inverse kinematics
Serial.println("Calculate angles:");
// Work out the length of an imaginery line from the arms shoulder to the x,y position and call it B
// using pythagoras theorem - length of b squared = x sqared + y squared
float B = sqrt ( ( x * x ) + ( y * y ) );
Serial.print(" B = "); Serial.println( B );
// Calculate the angle of the imaginary line from the x axis and call it QA
float QA = atan2 ( y , x );
Serial.print(" QA = "); Serial.println( QA );
// Calculate the angle from the imaginary line to the humerus (using cosine rule) and call it QB
float B_sq = B * B; // B squared
float tvala = hum_sq - uln_sq + B_sq ;
float tvalb = 2.0 * humerus * B;
float QB = acos ( tvala / tvalb );
Serial.print(" QB = "); Serial.println( QB );
// Calculate angle of shoulder by ading the two calculated angles together
float angleS = QA + QB;
Serial.print(" angleS = "); Serial.println( angleS );
// Calculate angle of elbow
// this is done using the cosine rule
tvala = hum_sq + uln_sq - B_sq ;
tvalb = 2.0 * humerus * ulna;
float angleE = acos ( tvala / tvalb );
Serial.print(" angleE = "); Serial.println( angleE );
// convert angles from radians to degrees
angleE = angleE * RAD_TO_DEG;
angleS = angleS * RAD_TO_DEG;
Serial.print("moving arm to x="); Serial.print(x); Serial.print(" Y="); Serial.println(y);
Serial.print(" angleS in degrees = "); Serial.println( angleS );
Serial.print(" angleE in degrees = "); Serial.println( angleE );
moveMotors(angleS,angleE); // move the arm
}
// ***************************************************************************
// Move motors to given angles
void moveMotors ( int s, int e ) {
int starget = s * ( countsperrev / 360 ); // convert target angle to motor steps
int etarget = e * ( countsperrev / 360 ); // convert target angle to motor steps
while ( shoulderPos != starget || elbowPos != etarget ) { // keep going until both angles are correct
if ( shoulderPos < starget ) {
moveStepper ( 0 , 1 );
delayMicroseconds(motorSpeed);
}
if ( shoulderPos > starget ) {
moveStepper ( 0 , -1 );
delayMicroseconds(motorSpeed);
}
if ( elbowPos < etarget ) {
moveStepper ( 1 , 1 );
delayMicroseconds(motorSpeed);
}
if ( elbowPos > etarget ) {
moveStepper ( 1 , -1 );
delayMicroseconds(motorSpeed);
}
}
allOff(); // all motor coils off
}
// ***************************************************************************
// move stepper motor one step
// motor = which motor 0 or 1 (0=shoulder 1=elbow)
// direct = which direction (1=clockwise, -1=anticlockwise)
void moveStepper(int motor, int direct)
{
if (motor == 0) {
// shoulder
shoulderStep = shoulderStep - direct; // increment current stepper motor step (0-7)
if (shoulderStep < 0) shoulderStep = 7;
if (shoulderStep > 7) shoulderStep = 0;
shoulderPos = shoulderPos + direct; // increment motor position flag
digitalWrite(motorSPin1, bitRead(lookup[shoulderStep], 0));
digitalWrite(motorSPin2, bitRead(lookup[shoulderStep], 1));
digitalWrite(motorSPin3, bitRead(lookup[shoulderStep], 2));
digitalWrite(motorSPin4, bitRead(lookup[shoulderStep], 3));
// also move elbow so it stays in position relative to shoulder arm
elbowStep = elbowStep + direct; // increment current stepper motor step (0-7)
if (elbowStep < 0) elbowStep = 7;
if (elbowStep > 7) elbowStep = 0;
digitalWrite(motorEPin1, bitRead(lookup[elbowStep], 0));
digitalWrite(motorEPin2, bitRead(lookup[elbowStep], 1));
digitalWrite(motorEPin3, bitRead(lookup[elbowStep], 2));
digitalWrite(motorEPin4, bitRead(lookup[elbowStep], 3));
}
else {
// elbow
elbowStep = elbowStep + direct; // increment current stepper motor step (0-7)
if (elbowStep < 0) elbowStep = 7;
if (elbowStep > 7) elbowStep = 0;
elbowPos = elbowPos + direct; // increment motor position flag
digitalWrite(motorEPin1, bitRead(lookup[elbowStep], 0));
digitalWrite(motorEPin2, bitRead(lookup[elbowStep], 1));
digitalWrite(motorEPin3, bitRead(lookup[elbowStep], 2));
digitalWrite(motorEPin4, bitRead(lookup[elbowStep], 3));
}
}
// ***************************************************************************
// all motor outputs off (make sure no coils left energized when not in use)
void allOff() {
digitalWrite(motorSPin1,0);
digitalWrite(motorSPin2,0);
digitalWrite(motorSPin3,0);
digitalWrite(motorSPin4,0);
digitalWrite(motorEPin1,0);
digitalWrite(motorEPin2,0);
digitalWrite(motorEPin3,0);
digitalWrite(motorEPin4,0);
}
// ***************************************************************************
// find arms position and move to default start location
void resetArm() {
// move elbow to stop then back to 90 degrees
for ( int i=0; i < ( countsperrev / 2.5); i++) {
moveStepper(1,-1);
delayMicroseconds(motorSpeed);
}
for ( int i=0; i < ( countsperrev / 4.4); i++) {
moveStepper(1,1);
delayMicroseconds(motorSpeed);
}
// reset arm location flags
shoulderPos = 1000; // current position of shoulder (in stepper motor steps)
elbowPos = 1000; // current position of elbow (in stepper motor steps)
allOff(); // turn all motor outputs off
}
// ***************************************************************************
// read serial input and move arm if anything recived
void moveArmFromSerial() {
const int serStrLen = 30;
char serInStr[ serStrLen ]; // array that will hold the serial input string
int input1, input2;
memset( serInStr, 0, sizeof(serInStr) ); // set it all to zero
delay(10); // wait a little for serial data
int i = 0;
while(Serial.available() && i<serStrLen ) {
serInStr[i] = Serial.read();
i++;
}
Serial.print ("Input = "); Serial.println( serInStr );
char* str = serInStr; // Save the remaining string for inputs.
input1 = atoi(str); // Next command is input1, capture and save it.
while( (*++str == ',') == 0 ); // find comma
*++str; // jump past comma
input2 = atoi(str); // Next command is input2, capture and save it.
delay(30); // Delay to settle the UART buffer.
Serial.print("Moving arm to "); Serial.print( input1 ); Serial.print( " , " ); Serial.println( input2 );
moveArm(input1,input2);
allOff(); // all motor outputs off
}
// ***************************************************************************
// end