Dear Forum Users,
I am working on a project that requires the use of a robotic arm to push buttons. The code that I have written is based on the Robot Arm Designer spreadsheet calculations that I found on this website. I have written it for use by an Arduino Uno microcontroller, which would actuate two servos (shoulder and elbow joints), mounted on a two linkage arm. The linkages are each 10 inches in the code, but this could be easily changed for a different arm length.
For the moment, the code is supposed to actuate based on commands from a computer in a terminal screen (pressing a number from 1 to 7) to have the arm push horizontally towards a button. The goal here is to make sure that the tip of the arm moves in a straight line. The robot that the arm will be mounted on will not be able to localize itself precisely enough to know how far away the button is, but the vertical position of the arm is certain because it is dictated by the height of the shoulder mount. Hence the need for the tip of the arm to move horizontally.
Now that I have my code written, I would like to see if it works. However, I have no servos to test it out. I would like to know if the code works before any servos I order get here so that I can perfect the code in the meantime. If anyone has the parts and the time to test out my code I would appreciate it.
Since I am a Mechanical Engineer and don't have a ton of experience coding, this might be a bit messy. Feel free to correct my code any way you see fit. I welcome constructive criticism.
Thanks!
Tom
tldr: I need help testing some Arduino code for a two-servo arm. It needs to move the tip of the arm in a straight line.
/* Arm Controller Code
*
* Adapted from the "Robot Arm Designer" spreadsheet design
* Produced by [url=http://www.societyofrobots.com]www.societyofrobots.com[/url]
*
* Customized for use at Carnegie Mellon University
* Tom Charley ... 2012
*/
/** Set up variables for equations **/
double upperarm = 10; // length of first arm segment
double forearm = 10; // length of second arm segment
double x; // horizontal position of arm tip
double y; // vertical position of arm tip
double c; // not sure yet what this stands for
double s; // not sure yet what this stands for
double k1; // upper arm plus parallel portion of forearm (I think...)
double k2; // portion of forearm (I think...)
double theta; // angle between vertical and upper arm (rad)
double psi; // angle between upper arm and forearm (rad)
double angle1; // theta in degrees
double angle2; // psi in degrees
double angleshoulder; // angle of shoulder servo converted to pulse width
double angleelbow; // angle of elbow servo converted to pulse width
double pi = 3.141592653589; // pi... yum
/** Set up microcontroller interface **/
int shoulder = 2; // shoulder servo
int elbow = 4; // elbow servo
int refreshTime = 20; // time (ms) between pulses (50Hz)
int moveServo; // raw user input
long lastPulse = 0; // recorded time (ms) of the last pulse
void setup()
{
pinMode(shoulder, OUTPUT); // set microcontroller pins to output
pinMode(elbow, OUTPUT); // set microcontroller pins to output
angleshoulder = 400; // give the servo a starting point (or it floats)
angleelbow = 400; // give the servo a starting point (or it floats)
Serial.begin(9600); // Tell it what to do in the Terminal screen
Serial.println(" Arduino Serial Servo Control");
Serial.println(" Press 0 to retract arm");
Serial.println("Press 1 through 7 for different button positions");
Serial.println();
}
/* This is the calculation that needs to be done for each position calculation:
*
* c = (square(x) + square(y) - square(upperarm) - square(forearm))/(2 * upperarm * forearm);
* s = sqrt(1 - square(c));
* k1 = upperarm + (forearm * c);
* k2 = forearm * s;
* theta = atan2(x,y) - atan2(k1,k2);
* angle1 = (180 * theta) / pi;
* angle2 = (180 * psi) / pi;
*/
void calculation()
{
c = (square(x) + square(y) - square(upperarm) - square(forearm))/(2 * upperarm * forearm);
s = sqrt(1 - square(c));
k1 = upperarm + (forearm * c);
k2 = forearm * s;
theta = atan2(x,y) - atan2(k1,k2);
angle1 = (180 * theta) / pi;
angle2 = (180 * psi) / pi;
angleshoulder = map(angle1, 0, 180, 400, 2200);
angleelbow = map(angle2, 0, 180, 400, 2200);
}
void actuate()
{
// pulse the servo every refreshTime with current pulseWidth
// this will hold the servo's position if unchanged, or move it if changed
if (millis() - lastPulse >= refreshTime) {
digitalWrite(shoulder, HIGH); // start the pulse
delayMicroseconds(angleshoulder); // pulse width
digitalWrite(shoulder, LOW); // stop the pulse
digitalWrite(elbow, HIGH); // start the pulse
delayMicroseconds(angleelbow); // pulse width
digitalWrite(elbow, LOW); // stop the pulse
lastPulse = millis(); // save the time of the last pulse
}
}
void loop()
{
// wait for serial input
if (Serial.available() > 0)
{
// read the incoming byte:
moveServo = Serial.read();
// In ASCII, '0' is 48, and the numbers go up sequentially to '7' at 55
if (moveServo == 48)
{
x = 0;
y = 10;
calculation;
actuate;
actuate;
actuate;
}
if (moveServo == 49)
{
x = 5;
y = 0;
calculation;
actuate;
actuate;
actuate;
x = 6;
y = 0;
calculation;
actuate;
actuate;
x = 7;
y = 0;
calculation;
actuate;
actuate;
x = 8;
y = 0;
calculation;
actuate;
actuate;
}
if (moveServo == 50) {
x = 5;
y = 1;
calculation;
actuate;
actuate;
actuate;
x = 6;
y = 1;
calculation;
actuate;
actuate;
x = 7;
y = 1;
calculation;
actuate;
actuate;
x = 8;
y = 1;
calculation;
actuate;
actuate;
}
if (moveServo == 51) {
x = 5;
y = 2;
calculation;
actuate;
actuate;
actuate;
x = 6;
y = 2;
calculation;
actuate;
actuate;
x = 7;
y = 2;
calculation;
actuate;
actuate;
x = 8;
y = 2;
calculation;
actuate;
actuate;
}
if (moveServo == 52) {
x = 5;
y = 3;
calculation;
actuate;
actuate;
actuate;
x = 6;
y = 3;
calculation;
actuate;
actuate;
x = 7;
y = 3;
calculation;
actuate;
actuate;
x = 8;
y = 3;
calculation;
actuate;
actuate;
}
if (moveServo == 53) {
x = 5;
y = 4;
calculation;
actuate;
actuate;
actuate;
x = 6;
y = 4;
calculation;
actuate;
actuate;
x = 7;
y = 4;
calculation;
actuate;
actuate;
x = 8;
y = 4;
calculation;
actuate;
actuate;
}
if (moveServo == 54) {
x = 5;
y = 5;
calculation;
actuate;
actuate;
actuate;
x = 6;
y = 5;
calculation;
actuate;
actuate;
x = 7;
y = 5;
calculation;
actuate;
actuate;
x = 8;
y = 5;
calculation;
actuate;
actuate;
}
if (moveServo == 55) {
x = 5;
y = 6;
calculation;
actuate;
actuate;
actuate;
x = 6;
y = 6;
calculation;
actuate;
actuate;
x = 7;
y = 6;
calculation;
actuate;
actuate;
x = 8;
y = 6;
calculation;
actuate;
actuate;
}
// print pulseWidth back to the Serial Monitor
Serial.print("Shoulder Pulse Width: ");
Serial.print(angleshoulder);
Serial.print("us "); // microseconds
Serial.print("Shoulder Angle: ");
Serial.print(angle1);
Serial.println("deg"); // degrees
Serial.print("Elbow Pulse Width: ");
Serial.print(angleelbow);
Serial.print("us "); // microseconds
Serial.print("Elbow Angle: ");
Serial.print(angle2);
Serial.println("deg"); // degrees
}
}
