Hello
I am making a robotic arm for a school project.
First, I created some simple code to test it, I have 4 POTs controlling each joint and a joystick for the wrist.
This all worked fine, there were no problems. (I guess this is my way of saying the electronics are fine)
#include <Servo.h>
Servo shoulder1_s;
Servo shoulder2_s;
Servo upperArm_s;
Servo elbow_s;
Servo wrist1_s;
Servo wrist2_s;
int pos1, pos2, pos3, pos4, pos5, pos6;
void setup() {
pinMode(13, OUTPUT);
pinMode(12, OUTPUT);
pinMode(11, OUTPUT);
pinMode(10, OUTPUT);
pinMode(9, OUTPUT);
pinMode(8, OUTPUT);
shoulder1_s.attach(13, 500, 2500);
shoulder2_s.attach(12, 500, 2550);
upperArm_s.attach(11, 550, 2400);
elbow_s.attach(10, 700, 2300);
wrist1_s.attach(9, 500, 2500);
wrist2_s.attach(8, 500, 2500);
}
void loop() {
pos1 = map(analogRead(A0), 0, 1023, 500, 2500);
pos2 = map(analogRead(A1), 0, 1023, 500, 2550);
pos3 = map(analogRead(A2), 0, 1023, 550, 2400);
pos4 = map(analogRead(A3), 0, 1023, 700, 2300);
pos5 = map(analogRead(A4), 0, 1023, 500, 2500);
pos6 = map(analogRead(A5), 0, 1023, 500, 2500);
shoulder1_s.writeMicroseconds(pos1);
shoulder2_s.writeMicroseconds(pos2);
upperArm_s.writeMicroseconds(pos3);
elbow_s.writeMicroseconds(pos4);
wrist1_s.writeMicroseconds(pos5);
wrist2_s.writeMicroseconds(pos6);
}
My next step is to do the inverse kinematics with this I wanted to set the servo with an angle, not microseconds but I wanted better resolution then you can do with the servo.write() in the servo library, I wanted to be able to use doubles with two decimals points for my angles. So I wrote a class Joint that had a servo object inside it as well as some functions to convert a double angle into microseconds.
Note that I won't be using POTs in the future to input positions into the writeAngle function that is why there is the rounding off line.
class joint {
private:
int angleMax; //upper limit of the servo
int angleMin; // lower limit of the servo
int secMax; // milisecond max
int secMin; // milisecond min
double angle; // current angle
double debug;
double ratio;
Servo servo; //servo object
public:
double getAngle() {
return angle;
}
void writeAngle(double pos) {
angle = floor(pos * 100 + 0.5) / 100; // rounds angle to 2 decimal places
angle = constrain(angle, angleMin, angleMax); //safty to stop angle being greater then where it should move
int value;
value = round(angle * ratio) + secMin; //maps the angle to miliseconds
//debug = value;
servo.writeMicroseconds(value); //writes milis to the servo
}
void writeMicroS(int pos) {
pos = constrain(pos, secMin, secMax); //safty to mkae sure the angle is safe
angle = floor((pos - secMin) / ratio * 100 + 0.5) / 100; //converts milis to anlge
servo.writeMicroseconds(pos);
}
double getDebug() {
return servo.readMicroseconds();
}
joint(byte pin, int min, int max, double startAngle, int pAngleMin = 0, int pAngleMax = 180) {
//set up for joint
angleMin = pAngleMin;
angleMax = pAngleMax;
secMin = min;
secMax = max;
servo.attach(pin, min, max);
writeAngle(startAngle);
ratio = double(secMax - secMin) / (angleMax - angleMin);
}
};
Here is the whole code:
#include "Servo.h"
#include "math.h"
double pos1, pos2, pos3, pos4;
// class joint: has a subclasses servo. tracks position of joint. allows for greater accuracy control
class joint {
private:
int angleMax; //upper limit of the servo
int angleMin; // lower limit of the servo
int secMax; // milisecond max
int secMin; // milisecond min
double angle; // current angle
double debug;
double ratio;
Servo servo; //servo object
public:
double getAngle() {
return angle;
}
void writeAngle(double pos) {
angle = floor(pos * 100 + 0.5) / 100; // rounds angle to 2 decimal places
angle = constrain(angle, angleMin, angleMax); //safty to stop angle being greater then where it should move
int value;
value = round(angle * ratio) + secMin; //maps the angle to miliseconds
//debug = value;
servo.writeMicroseconds(value); //writes milis to the servo
}
void writeMicroS(int pos) {
pos = constrain(pos, secMin, secMax); //safty to mkae sure the angle is safe
angle = floor((pos - secMin) / ratio * 100 + 0.5) / 100; //converts milis to anlge
servo.writeMicroseconds(pos);
}
double getDebug() {
return servo.readMicroseconds();
}
joint(byte pin, int min, int max, double startAngle, int pAngleMin = 0, int pAngleMax = 180) {
//set up for joint
angleMin = pAngleMin;
angleMax = pAngleMax;
secMin = min;
secMax = max;
servo.attach(pin, min, max);
writeAngle(startAngle);
ratio = double(secMax - secMin) / (angleMax - angleMin);
}
};
//set up joints
joint shoulderIn(13, 500, 2500, 0);
joint ShoulderOut(12, 500, 2550, 90);
joint UpperArm(11, 550, 2400, 0);
joint Elbow(10, 700, 2300, 0, 0, 145);
joint WristIn(9, 500, 2500, 90);
joint WristOut(8, 500, 2500, 90);
//takes 6 doubles as servo position, wrist deafults to last wrist angle
void SetServos(double pos1, double pos2, double pos3, double pos4, double pos5 = double(WristIn.getAngle()
), double pos6 = double(WristOut.getAngle())) {
shoulderIn.writeAngle(pos1);
ShoulderOut.writeAngle(pos2);
UpperArm.writeAngle(pos3);
Elbow.writeAngle(pos4);
WristIn.writeAngle(pos5);
WristOut.writeAngle(pos6);
}
void setup() {
Serial.begin(9600);
}
void loop() {
// maps POT input to angles
pos1 = map(analogRead(A0), 0, 1023, 0, 180 * 100);
pos1 = pos1 / 100;
pos2 = map(analogRead(A1), 0, 1023, 0, 180 * 100);
pos2 = pos2 / 100;
pos3 = map(analogRead(A2), 0, 1023, 0, 180 * 100);
pos3 = pos3 / 100;
pos4 = map(analogRead(A3), 0, 1023, 0, 145 * 100);
pos4 = pos4 / 100;
SetServos(pos1, pos2, pos3, pos4);
Serial.print(shoulderIn.getDebug());
Serial.print(" ");
Serial.print(ShoulderOut.getDebug());
Serial.print(" ");
Serial.print(UpperArm.getDebug());
Serial.print(" ");
Serial.print(Elbow.getDebug());
Serial.print(" ");
Serial.print(WristIn.getDebug());
Serial.print(" ");
Serial.println(WristOut.getDebug());
}
When I use this program my arm moves jaggedly to a position and then jitters around there quite harshly. When I adjust my pots it doesn't respond. In my debug the of servo.read the outputs are all as I would expect. eg 2500 when the pot is turned all the way for the first servo.
My guess at the problem is that there is something wrong with instantiating a servo object in my joint class. As the first program without a class worked fine
So I did some experiments, I created only one joint object when I did this nothing happened and the servo wasn't holding a position, I could back drive it.
Then I tried with two joint objects, it did a similar thing as when I had all the objects. however, I was kind of able to position the second servo although it was very delayed.
Looking forward to hearing some suggestions
Thanks in advance