The rest:
void CenterServos() // center the servos on powerup
{
Lleg.write(LLcenter); // tell servo to go to position in variable 'center'
Rleg.write(RLcenter); // tell servo to go to position in variable 'center'
Lfoot.write(LFcenter); // tell servo to go to position in variable 'center'
Rfoot.write(RFcenter); // tell servo to go to position in variable 'center'
Neck.write(Neckcenter); // tell servo to go to position in variable 'center'
delay(100); // waits 100ms for the servos to reach the position
}
void BuzzerBeep(){ //beep the buzzer on event
digitalWrite(BuzzerPin, HIGH);
chThdSleepMilliseconds(100);
digitalWrite(BuzzerPin, LOW);
}
//Servo Primatives
void TiltRightUp(byte ang, byte sp){
//tilt right up
for (int i=0; i<=ang; i+=5){
Lfoot.write(LFcenter+i);
Rfoot.write(RFcenter+i);
chThdSleepMilliseconds(sp);
}
}
void TiltRightDown(byte ang, byte sp){
//tilt right down
for (int i=ang; i>0; i-=5){
Lfoot.write(LFcenter+i);
Rfoot.write(RFcenter+i);
chThdSleepMilliseconds(sp);
}
}
void TiltLeftUp(byte ang, byte sp){
//tilt left up
for (int i=0; i<=ang; i+=5){
Lfoot.write(LFcenter-i);
Rfoot.write(RFcenter-i);
chThdSleepMilliseconds(sp);
}
}
void TiltLeftDown(byte ang, byte sp){
//tilt left down
for (int i=ang; i>0; i-=5){
Lfoot.write(LFcenter-i);
Rfoot.write(RFcenter-i);
chThdSleepMilliseconds(sp);
}
}
void LeftFootUp(char ang, byte sp){
//tilt left up
for (int i=0; i<=ang; i+=5){
Lfoot.write(LFcenter-i);
chThdSleepMilliseconds(sp);
}
}
void LeftFootDown(byte ang, byte sp){
//tilt left down
for (int i=ang; i>0; i-=5){
Lfoot.write(LFcenter-i);
chThdSleepMilliseconds(sp);
}
}
void RightFootUp(byte ang, byte sp){
//tilt right up
for (int i=0; i<=ang; i+=5){
Rfoot.write(RFcenter+i);
chThdSleepMilliseconds(sp);
}
}
void RightFootDown(byte ang, byte sp){
//tilt right down
for (int i=ang; i>0; i-=5){
Rfoot.write(RFcenter+i);
chThdSleepMilliseconds(sp);
}
}
void SwingRight(byte ang, byte sp){
//swing right
for (int i=0; i<=ang; i+=5){
Lleg.write(LLcenter-i);
Rleg.write(RLcenter-i);
chThdSleepMilliseconds(sp);
}
}
void SwingRcenter(byte ang, byte sp){
//swing r->center
for (int i=ang; i>0; i-=5){
Lleg.write(LLcenter-i);
Rleg.write(RLcenter-i);
chThdSleepMilliseconds(sp);
}
}
void SwingLeft(byte ang, byte sp){
//swing left
for (byte i=0; i<=ang; i=i+5){
Lleg.write(LLcenter+i);
Rleg.write(RLcenter+i);
chThdSleepMilliseconds(sp);
}
}
void SwingLcenter(byte ang, byte sp){
//swing l->center
for (byte i=ang; i>0; i=i-5){
Lleg.write(LLcenter+i);
Rleg.write(RLcenter+i);
chThdSleepMilliseconds(sp);
}
}
void RightLegIn(byte ang, byte sp){
//swing right
for (int i=0; i<=ang; i+=5){
Rleg.write(RLcenter-i);
chThdSleepMilliseconds(sp);
}
}
void RightLegIcenter(byte ang, byte sp){
//swing r->center
for (int i=ang; i>0; i-=5){
Rleg.write(RLcenter-i);
chThdSleepMilliseconds(sp);
}
}
void RightLegOut(byte ang, byte sp){
//swing right
for (int i=0; i<=ang; i+=5){
Rleg.write(RLcenter+i);
chThdSleepMilliseconds(sp);
}
}
void RightLegOcenter(byte ang, byte sp){
//swing r->center
for (int i=ang; i>0; i-=5){
Rleg.write(RLcenter+i);
chThdSleepMilliseconds(sp);
}
}
void LeftLegIn(byte ang, byte sp){
//swing left
for (byte i=0; i<=ang; i=i+5){
Lleg.write(LLcenter+i);
chThdSleepMilliseconds(sp);
}
}
void LeftLegIcenter(byte ang, byte sp){
//swing l->center
for (byte i=ang; i>0; i=i-5){
Lleg.write(LLcenter+i);
chThdSleepMilliseconds(sp);
}
}
void LeftLegOut(byte ang, byte sp){
//swing left
for (byte i=0; i<=ang; i=i+5){
Lleg.write(LLcenter-i);
chThdSleepMilliseconds(sp);
}
}
void LeftLegOcenter(byte ang, byte sp){
//swing l->center
for (byte i=ang; i>0; i=i-5){
Lleg.write(LLcenter-i);
chThdSleepMilliseconds(sp);
}
}
void NeckLeft(byte ang, byte sp){
//swing left
for (int i=0; i<=ang; i+=5){
Neck.write(Neckcenter+i);
chThdSleepMilliseconds(sp);
}
}
void NeckRight(byte ang, byte sp){
//swing right
for (int i=0; i<=ang; i+=5){
Neck.write(Neckcenter-i);
chThdSleepMilliseconds(sp);
}
}
void NeckIcenter(byte ang, byte sp){
//swing neck->center
for (byte i=ang; i>0; i=i-5){
Neck.write(Neckcenter+i);
chThdSleepMilliseconds(sp);
}
}
void NeckOcenter(byte ang, byte sp){
//swing neck->center
for (byte i=ang; i>0; i=i-5){
Neck.write(Neckcenter-i);
chThdSleepMilliseconds(sp);
}
}
//Walk Functions
void Forward(byte Steps, byte Speed){
digitalWrite(EnableServo,HIGH);
TiltRightUp(tAngle, Speed);
for (byte j=0; j<Steps; ++j){
SwingRight(sAngle, Speed);
TiltRightDown(tAngle, Speed);
TiltLeftUp(tAngle, Speed);
SwingRcenter(sAngle, Speed);
SwingLeft(sAngle, Speed);
TiltLeftDown(tAngle, Speed);
TiltRightUp(tAngle, Speed);
SwingLcenter(sAngle, Speed);
}
TiltRightDown(tAngle, Speed);
digitalWrite(EnableServo,LOW);
}
void Reverse(byte Steps, byte Speed){
digitalWrite(EnableServo,HIGH);
TiltLeftUp(tAngle, Speed);
for (byte j=0; j<Steps; ++j){
SwingRight(sAngle, Speed);
TiltLeftDown(tAngle, Speed);
TiltRightUp(tAngle, Speed);
SwingRcenter(sAngle, Speed);
SwingLeft(sAngle, Speed);
TiltRightDown(tAngle, Speed);
TiltLeftUp(tAngle, Speed);
SwingLcenter(sAngle, Speed);
}
TiltLeftDown(tAngle, Speed);
digitalWrite(EnableServo,LOW);
}
void TurnLeft(byte Steps, byte Speed){
digitalWrite(EnableServo,HIGH);
TiltLeftUp(uAngle, Speed);
chThdSleepMilliseconds(20);
for (byte j=0; j<Steps; ++j){
LeftLegIn(sAngle, Speed);
TiltLeftDown(uAngle, Speed);
TiltRightUp(uAngle, Speed);
chThdSleepMilliseconds(20);
LeftLegIcenter(sAngle, Speed);
RightLegOut(sAngle, Speed);
TiltRightDown(uAngle, Speed);
TiltLeftUp(uAngle, Speed);
chThdSleepMilliseconds(20);
RightLegOcenter(sAngle, Speed);
}
TiltLeftDown(uAngle, Speed);
digitalWrite(EnableServo,LOW);
}
void TurnRight(byte Stps, byte Speed){
digitalWrite(EnableServo,HIGH);
TiltRightUp(uAngle, Speed);
chThdSleepMilliseconds(20);
for (byte f=0; f<=Stps; ++f){
RightLegIn(sAngle, Speed);
TiltRightDown(uAngle, Speed);
TiltLeftUp(uAngle, Speed);
chThdSleepMilliseconds(20);
RightLegIcenter(sAngle, Speed);
LeftLegOut(sAngle, Speed);
TiltLeftDown(uAngle, Speed);
TiltRightUp(uAngle, Speed);
chThdSleepMilliseconds(20);
LeftLegOcenter(sAngle, Speed);
}
TiltRightDown(uAngle, Speed);
digitalWrite(EnableServo,LOW);
}