Arduino uno 6dof
- This topic has 2 voices and 15 replies.
-
Posts
-
Hi,
as a Newbie I’m struggeling with a code for Arduino uno and six servos connected to pins 3,5,6,9,10,11.
I’v build Steward platform, but I can’ get servos moving with code bellow
Any Ideas, thank you in advance#include
const float pi = 3.14159,
theta_r = radians(48.0),
theta_p = radians(23.2),
theta_s[] = { -pi / 3, 2 * pi / 3, pi, 0, pi / 3, -2 * pi / 3},
RD = 2.395,
PD = 3.3,
L1 = 1.0,
L2 = 4.72,
z_home = 4.25,
servo_min = 3.5,
servo_max = 5.5,
servo_mult = 6,
p[2][6] = {{PD * cos(pi / 6 + theta_p), PD * cos(pi / 6 - theta_p), PD * cos(-(pi / 2 - theta_p)), -PD * cos(-(pi / 2 - theta_p)), -PD * cos(pi / 6 - theta_p), -PD * cos(pi / 6 + theta_p)},
{PD * sin(pi / 6 + theta_p), PD * sin(pi / 6 - theta_p), PD * sin(-(pi / 2 - theta_p)), PD * sin(-(pi / 2 - theta_p)), PD * sin(pi / 6 - theta_p), PD * sin(pi / 6 + theta_p)}
},
re[2][6] = {{RD * cos(pi / 6 + theta_r), RD * cos(pi / 6 - theta_r), RD * cos(-(pi / 2 - theta_r)), -RD * cos(-(pi / 2 - theta_r)), -RD * cos(pi / 6 - theta_r), -RD * cos(pi / 6 + theta_r)},
{RD * sin(pi / 6 + theta_r), RD * sin(pi / 6 - theta_r), RD * sin(-(pi / 2 - theta_r)), RD * sin(-(pi / 2 - theta_r)), RD * sin(pi / 6 - theta_r), RD * sin(pi / 6 + theta_r)}
};
/*
theta_r = angle between attachment points
theta_p = angle between rotation points
theta_s = orientation of the servos
RD = distance to end effector attachment points
PD = distance to servo rotation points
L1 = servo arm length
L2 = connecting arm length
z_home = default z height with servo arms horizontal
servo_min = lower limit for servo arm angle
servo_max = upper limit for servo arm angle
servo_mult = multiplier to convert to milliseconds
p = location of servo rotation points in base frame [x/y][1-6]
re = location of attachment points in end effector frame [x/y][1-6]
*/
const int servo_pin[] = {9, 3, 5, 11, 6, 10};
const int servo_zero[6] = {1710, 1280, 1700, 1300, 1680, 1300};
/*
servo_pin = servo pin assignments,
servo_zero = zero angles for each servo (horizontal)
*/
Servo servo[6];
/*
Servos 0, 2, 4: reversed (+ = down, - = up)
Servos 1, 3, 5: normal (+ = up, - = down)
*/
void setup()
{
//Serial.begin(9600);
for (int i = 0; i < 6; i++)
{
servo.attach(servo_pin);
servo.writeMicroseconds(servo_zero);
}
delay(1000);
}
void loop()
{
static float pe[6] = {0, 0, 0, radians(0), radians(0), radians(0)}, theta_a[6], servo_pos[6], q[3][6], r[3][6], dl[3][6], dl2[6];
}
/*
pe = location and orientation of end effector frame relative to the base frame [sway, surge, heave, pitch, roll, yaw)
theta_a = angle of the servo arm
servo_pos = value written to each servo
q = position of lower mounting point of connecting link [x,y,x][1-6]
r = position of upper mounting point of connecting link
dl = difference between x,y,z coordinates of q and r
dl2 = distance between q and r
*/
I prefer to work with vectors and think in dot products, cross products, etc.
I can draw the model and how to build the equations easier that way.It’s connecting servos to pins 1-6 (inclusive) and then moving them to middle position. I used this code to center the servos so then I could attach the servo arms and know they’re *pretty close* to horizontal.
change *30 to *20 or *10… the smaller the number, the smaller the oscillation.
I think it’s a good plan. Post updates as you go, I want to learn more about cool term.
erratically how? Can you make a video please?
Something is sending random PWM signals to your servos. What happens if your arduino sends only (MAX+MIN)/2 to all the servos?
- You must be logged in to reply to this topic.
