I’m trying to make a code to move my BRAT’s servos. I’m a little unsure of if I’m on the right track and I don’t want to continue on this path without having wiser programmers having a look see. Thanks!
The code is attached and it’s heavily annotated. It should speak for itself.
Group_move_walk_sketch.ino (4.49 KB)
Looking good,
'From what I saw it looks like you might be on a reasonable track. If it were me, I would use arrays and loop through the arrays to process each of the servos instead of having separate section of code for each servo.
For what it is worth, I have my own version of the Servo library, I call ServoEx, which tries to emulate the group move capabilities of the SSC-32. I have not checked it out in awhile to see if there were changes in the recent versions of the Servo library, that I should pull back in… If you are interested, this library is up on github as part of my Phoenix In parts project: (github.com/KurtE/Arduino_Phoenix_Parts)
Not sure if it would help at all, but I believe I posted a version of the brat using a PS2 back in the old thread: post-p77519
Kurt
Thank you Kurte, I appreciate you looking at it. But I must ask for further help haha.
So I ran a test of servo RA and it worked, so I finished adding in the calculation segments for the other servos. But now, no matter what angle I put in for my function the servos always go the same direction and ALL the way until they collide with the frame. This happens every time and I can’t seem to find whats wrong. Could you (or anyone with the time and a kind heart) take a look and tell me if they see something?
Also, I know that my servos are centered on my robot because I took care of that while
//This code was written by Ensign Justin Shelton, USN.
//Designed to control groups of servos in sequence.
// 6/14/2014
#include <Servo.h>
Servo Servo_RH; //Right hip
Servo Servo_RK; //Right knee
Servo Servo_RA; //Right ankle
Servo Servo_LH; //Left hip
Servo Servo_LK; //Left knee
Servo Servo_LA; //Left ankle
int RHLastPosition = -1; //Right hip last position
int RKLastPosition = -1; //Right knee last position
int RALastPosition = -1; //Right ankle last position
int LHLastPosition = -1; //Left hip last position
int LKLastPosition = -1; //Left knee last position
int LALastPosition = -1; //Left ankle last position
bool runOnce=false;
int LA = 0; //new position of the Left ankle
int LK = 0; //new position of the Left knee
int LH = 0; //new position of the Left hip
int RA = 0; //new position of the Right ankle
int RK = 0; //new position of the Right knee
int RH = 0; //new position of the Right hip
//This is a function that tells each servo to move to a new positio(an angle)
//int TimeChange indicates how long the servo has to get to it's new position.
//Example:
// ServoTimedMove(45, 45, 45, 45, 45, 45, 200);
// The would make the left ankle move to 45 in 200ms, then the Left knee to 45 in 200ms, and so on.
void ServoTimedMove(int RA, int RK, int RH, int LH, int LK, int LA, int TimeChange)
{
{
//Of course, we cannot do a timed move for a servo if we don't know it's position....
//so this means that for each servo, we need to be give the servo some differences in last
//position and the new position. We start with the right ankle.
if ((TimeChange != 0) && (RALastPosition != 1))
{
int DeltaRA = RA - RALastPosition; //The total degree change for RA is the new position(RA) minus RAlastposition
int CycleTime = 20;
int CountOfCycles = TimeChange / 20; //this is because 20ms is the natural recycle time for a servo.
int RADeltaPerCycle = DeltaRA/CountOfCycles;
if (RADeltaPerCycle == 0) //This is for when the changes per cycle is equal to zero
{
// If there isn't enough change to do it, then....
RADeltaPerCycle = (DeltaRA > 0)?1: -1; //this assumes there is a change per cycle.
CountOfCycles = abs(DeltaRA); //As you can guess, this takes the absolute value of the change.
CycleTime = TimeChange/CountOfCycles;
}
int RA = RALastPosition;
while (CountOfCycles--)
{
RALastPosition += RADeltaPerCycle;
Servo_RA.write(RA);
delay(CycleTime);
}
Servo_RA.write(RA);
RALastPosition = RA;
}
}
//This is for the Right knee
{
if ((TimeChange != 0) && (RKLastPosition != 1))
{
int DeltaRK = RK - RKLastPosition; //The total degree change for RA is the new position(RA) minus RAlastposition
int CycleTime = 20;
int CountOfCycles = TimeChange / 20; //this is because 20ms is the natural recycle time for a servo.
int RKDeltaPerCycle = DeltaRK/CountOfCycles;
if (RKDeltaPerCycle == 0) //This is for when the changes per cycle is equal to zero
{
// If there isn't enough change to do it, then....
RKDeltaPerCycle = (DeltaRK > 0)?1: -1; //this assumes there is a change per cycle.
CountOfCycles = abs(DeltaRK); //As you can guess, this takes the absolute value of the change.
CycleTime = TimeChange/CountOfCycles;
}
int RK = RKLastPosition;
while (CountOfCycles--)
{
RKLastPosition += RKDeltaPerCycle;
Servo_RK.write(RK);
delay(CycleTime);
}
Servo_RK.write(RK);
RKLastPosition = RK;
}
}
//This is for the Right hip
{
if ((TimeChange != 0) && (RHLastPosition != 1))
{
int DeltaRH = RH - RHLastPosition; //The total degree change for RA is the new position(RA) minus RAlastposition
int CycleTime = 20;
int CountOfCycles = TimeChange / 20; //this is because 20ms is the natural recycle time for a servo.
int RHDeltaPerCycle = DeltaRH/CountOfCycles;
if (RHDeltaPerCycle == 0) //This is for when the changes per cycle is equal to zero
{
// If there isn't enough change to do it, then....
RHDeltaPerCycle = (DeltaRH > 0)?1: -1; //this assumes there is a change per cycle.
CountOfCycles = abs(DeltaRH); //As you can guess, this takes the absolute value of the change.
CycleTime = TimeChange/CountOfCycles;
}
int RH = RHLastPosition;
while (CountOfCycles--)
{
RHLastPosition += RHDeltaPerCycle;
Servo_RH.write(RH);
delay(CycleTime);
}
Servo_RH.write(RH);
RHLastPosition = RH;
}
}
//This is for the Left hip
{
if ((TimeChange != 0) && (LHLastPosition != 1))
{
int DeltaLH = LH - LHLastPosition; //The total degree change for RA is the new position(RA) minus RAlastposition
int CycleTime = 20;
int CountOfCycles = TimeChange / 20; //this is because 20ms is the natural recycle time for a servo.
int LHDeltaPerCycle = DeltaLH/CountOfCycles;
if (LHDeltaPerCycle == 0) //This is for when the changes per cycle is equal to zero
{
// If there isn't enough change to do it, then....
LHDeltaPerCycle = (DeltaLH > 0)?1: -1; //this assumes there is a change per cycle.
CountOfCycles = abs(DeltaLH); //As you can guess, this takes the absolute value of the change.
CycleTime = TimeChange/CountOfCycles;
}
int LH = LHLastPosition;
while (CountOfCycles--)
{
LHLastPosition += LHDeltaPerCycle;
Servo_LH.write(LH);
delay(CycleTime);
}
Servo_LH.write(LH);
LHLastPosition = LH;
}
}
//This is for the Left knee
{
if ((TimeChange != 0) && (LKLastPosition != 1))
{
int DeltaLK = LK - LKLastPosition; //The total degree change for RA is the new position(RA) minus RAlastposition
int CycleTime = 20;
int CountOfCycles = TimeChange / 20; //this is because 20ms is the natural recycle time for a servo.
int LKDeltaPerCycle = DeltaLK/CountOfCycles;
if (LKDeltaPerCycle == 0) //This is for when the changes per cycle is equal to zero
{
// If there isn't enough change to do it, then....
LKDeltaPerCycle = (DeltaLK > 0)?1: -1; //this assumes there is a change per cycle.
CountOfCycles = abs(DeltaLK); //As you can guess, this takes the absolute value of the change.
CycleTime = TimeChange/CountOfCycles;
}
int LK = LKLastPosition;
while (CountOfCycles--)
{
LKLastPosition += LKDeltaPerCycle;
Servo_LK.write(LK);
delay(CycleTime);
}
Servo_LK.write(LK);
LHLastPosition = LK;
}
}
//This is for the Left ankle
{
if ((TimeChange != 0) && (LALastPosition != 1))
{
int DeltaLA = LA - LALastPosition; //The total degree change for RA is the new position(RA) minus RAlastposition
int CycleTime = 20;
int CountOfCycles = TimeChange / 20; //this is because 20ms is the natural recycle time for a servo.
int LADeltaPerCycle = DeltaLA/CountOfCycles;
if (LADeltaPerCycle == 0) //This is for when the changes per cycle is equal to zero
{
// If there isn't enough change to do it, then....
LADeltaPerCycle = (DeltaLA > 0)?1: -1; //this assumes there is a change per cycle.
CountOfCycles = abs(DeltaLA); //As you can guess, this takes the absolute value of the change.
CycleTime = TimeChange/CountOfCycles;
}
int LA = LALastPosition;
while (CountOfCycles--)
{
LALastPosition += LADeltaPerCycle;
Servo_LA.write(LA);
delay(CycleTime);
}
Servo_LA.write(LA);
LALastPosition = LA;
}
}
}
//This is a test of the program
void Test()
{
//ServoTimedMove( RA, RK, RH, LH, LK, LK, TIME)
ServoTimedMove( 45, 45, 45, 45, 45, 45, 500);
//So, theoretically if all goes well all the motors will whip to 90 degrees in 300ms each.
}
void DetachServos()
{
Servo_LH.detach();
Servo_LK.detach();
Servo_LA.detach();
Servo_RH.detach();
Servo_RK.detach();
Servo_RA.detach();
}
void setup()
{
Servo_LH.attach(4);
Servo_LK.attach(3);
Servo_LA.attach(2);
Servo_RH.attach(12);
Servo_RK.attach(11);
Servo_RA.attach(10);
}
void loop()
{
if (!runOnce)
{
Test();
delay(1000);
DetachServos();
runOnce=1; //Set true so the program doesn't run again
}
}
Hey, Varminthunter
My Arduino code is not that efficient but here is my code, the code makes the robot walk forward. This is a static walk.
Hope you can find use for my code!
Good Luck 
P.S. Also I didn’t have time to edit some useless comments which are mostly tips of where I left on when I have to go.
Arduino Code:
[code]#include <Servo.h>
Servo la; // create servo object, left ankle
Servo ra; // create servo object, right ankle
Servo lh; // left hip
Servo rh; // right hip
Servo rt;
Servo lt;
int dly; // delay value
int vla; // servo value
int vra; //servo value
int vlh; // servo value
int vrh; // servo value
int pos = 90;
void setup() {
ra.attach(2); // attaches servopin 6
la.attach(5); //attaches servopin 8
lh.attach(4); // attaches servopin 5
rh.attach(7); // attaches servopin 7
rt.attach(10);
lt.attach(9);
dly = 500; // sets delay value
la.write(105);
ra.write(90);
lh.write(120);
rh.write(100);
rt.write(80);
lt.write(80);
delay(2000); //finished setup for standing
// now lets bend right leg
//move right thigh now
rt.write(85);
rh.write(105);
//delay(dly);
rt.write(90);
rh.write(110);
//delay(dly);
rt.write(95);
rh.write(115);
//delay(dly);
rt.write(100);
//delay(dly);
rt.write(105);
//delay(dly);
rt.write(110);
delay(dly);
}
void loop()
{
ra.write(100); //going to lift up left leg
la.write(110);
//delay(dly);
ra.write(110);
la.write(120);
//delay(dly);
ra.write(115 );
la.write(130);
//delay(dly);
ra.write(120);
la.write(140);
//delay(dly);
ra.write(125);
la.write(150);
//delay(dly);
la.write(165);
//delay(dly);
//la.write(170);
//delay(dly);
//la.write(180);
delay(1000);/////////////lifted leg up //////////////
la.write(170);
delay(dly);
la.write(160);
delay(dly);
la.write(150);
delay(dly);
la.write(140);
delay(dly);
la.write(130);
delay(500); // evened out the left foot so leg is completly off the gound
//now to move right forward
rt.write(100);
rh.write(110);
delay(dly);
rt.write(90);
rh.write(100);
delay(dly);
rt.write(80);
rh.write(90);
delay(dly);
rt.write(70);
rh.write(80);
delay(dly);
rh.write(70);
//now bend the right leg forward
rt.write(60);
delay(dly);
rt.write(50);
delay(dly);
rt.write(40);
delay(dly);
rt.write(30);
delay(dly); //leg is bent far enough
//move left leg forward
lh.write(110);
lt.write(70);
delay(dly);
lh.write(100);
lt.write(60);
delay(dly);
lh.write(90);
lt.write(50);
delay(dly);
lh.write(80);
lt.write(40);
delay(dly);
lh.write(70);
lt.write(30);
delay(dly);
lh.write(60);
delay(dly);
lh.write(50);
delay(dly);
lh.write(40);
delay(dly);// now to rest the foot of the left leg on ground we have to set the right ankle to 90
ra.write(120);
la.write(120);
delay(dly);
ra.write(110);
la.write(100);
delay(dly);
ra.write(100);
delay(dly);
ra.write(90);
delay(dly); //finshed moving the leg forward now let’s move the other leg!!!
// move right leg forward
// done… I finshed
// im kidding… let’s code…
lh.write(50);
lt.write(40);
delay(dly);
lh.write(60);
lt.write(50);
delay(dly);
lh.write(70);
lt.write(60);
delay(dly);
lh.write(80);
delay(dly);
// move left ankle two reptition after so the it shifts the movement onto the left leg
ra.write(80);
//delay(dly);
ra.write(70);
//delay(dly);
ra.write(60);
la.write(100);
//delay(dly);
ra.write(40);
la.write(90);
//delay(dly);
la.write(80);
ra.write(30);
//delay(dly);
la.write(75); ////just edited this spot
ra.write(20);
//delay(dly);
//ra.write(10);
//la.write(65);
//delay(dly);
//still leg isn’t off the ground so we need to squat the left leg down just alittle
delay(1000); ////////////////// YES, we got both legs off the ground, changing the left hips location can means alot. One degree with 6 included degrees of freedom (6DOF) means everything. But we I did it. GG…
// now even out the right foot so its not touching the ground///////////////////////////////////////
ra.write(30);
delay(dly);
ra.write(40);
delay(dly);
ra.write(50);
delay(dly);
ra.write(60);
delay(dly);
ra.write(70);
delay(dly);
ra.write(80);
delay(dly);
ra.write(90);
delay(1000); // now we go the whole right side of the leg off the ground
// move the left leg forward
rh.write(80);
rt.write(30);
delay(dly);
rh.write(90);
rt.write(40);
delay(dly);
rh.write(100);
rt.write(50);
delay(dly);
rh.write(110);
rt.write(60);
delay(dly);
rt.write(70);
rh.write(120);
delay(dly);
rt.write(80);
rh.write(130);
delay(dly);
rt.write(90);
rh.write(140);
delay(dly);
rt.write(100);
delay(dly);
rt.write(110);
lt.write(70); ///////////////////////////////
lh.write(90); // FIX THE LENGTH OF THE //
delay(dly); // STRIDE TO GO FURTHER //
lh.write(100);// {{{NICE BOX}}} //
lt.write(80); ///////////////////////////////
delay(dly);
lh.write(110);
lt.write(90);
delay(dly);
lh.write(120);
lt.write(100);
delay(dly);
lh.write(130);
lt.write(110);
delay(dly);
lt.write(120);
lh.write(140);
delay(dly);
lt.write(130);
lh.write(150);
delay(dly);
la.write(70);
delay(dly);
la.write(80);
delay(dly);
la.write(90);
delay(dly);
la.write(100);
delay(dly);
la.write(105);
rt.write(110);
delay(dly);
lt.write(120);
lh.write(140);
delay(dly);
rt.write(100);
rh.write(130);
delay(dly);
rt.write(90);
rh.write(120);
delay(1000);
// now we got to prepare the the robot to repeat the begining code with a few pieces of code. Basically connecting the code back to the beginning.
}[/code]
Thanks, seeing this code was very helpful! It also helped me realized the importance of Arrays haha. Gotta find a good means of studying those.