/* ATLAS HEXAPOD 3 servo 6 leg robot control Cobbled together by Jim Watt www.clockworkrobot.com */
int servoPin1 = 0; // Control pin for middle servo motor int servoPin2 = 1; // Control pin for servo motor int servoPin3 = 2; // Control pin for servo motor
int LeftFeeler = 3; // Left Feeler int RightFeeler = 4; // Right Feeler
int RightLED = 8; // Right LED int LeftLED = 9; // Left LED
// Servo 1, middle legs int MinPulseS1 = 1390; // Minimum servo position. 1400 default. 900 is absolute min int MidPulseS1 = 1570; // Middle servo position. 1550 default int MaxPulseS1 = 1750; // Maximum servo position 1700 default. 21100 is absolute max
// Servo 2 and 3, outer legs int MinPulseS23 = 1100; // Minimum servo position. 1100 defauls. int MidPulseS23 = 1400; // Middle servo position. 1400 defauls. int MaxPulseS23 = 1700; // Maximum servo position. 1700 defauls.
int refreshTime = 20; // the time needed in between pulses int pulse = 1100; // servo pulse length in Microseconds int rate = 20; // speed to move the servo
int Feeler = 0; // feeler flag
void LeftPark() { for (int pulse=0; pulse <= 30; pulse=pulse+1){ digitalWrite(servoPin3, HIGH); // Turn the motor on delayMicroseconds(MidPulseS23); // Length of the pulse sets the motor position digitalWrite(servoPin3, LOW); // Turn the motor on delay(refreshTime); } }
void RightPark() { for (int pulse=0; pulse <= 30; pulse=pulse+1){ digitalWrite(servoPin2, HIGH); // Turn the motor on delayMicroseconds(MidPulseS23); // Length of the pulse sets the motor position digitalWrite(servoPin2, LOW); // Turn the motor on delay(refreshTime); } }
void MidPark() { for (int pulse=0; pulse <= 30; pulse=pulse+1){ digitalWrite(servoPin1, HIGH); // Turn the motor on delayMicroseconds(MidPulseS1); // Length of the pulse sets the motor position digitalWrite(servoPin1, LOW); // Turn the motor off delay(refreshTime); } }
void MidLeftOut() { for (int pulse=MidPulseS1; pulse <= MaxPulseS1; pulse=pulse+rate){ digitalWrite(servoPin1, HIGH); // Turn the motor on delayMicroseconds(pulse); // Length of the pulse sets the motor position digitalWrite(servoPin1, LOW); // Turn the motor off delay(refreshTime); } }
void MidLeftRet() { for (int pulse=MaxPulseS1; pulse >= MidPulseS1; pulse=pulse-rate){ digitalWrite(servoPin1, HIGH); // Turn the motor on delayMicroseconds(pulse); // Length of the pulse sets the motor position digitalWrite(servoPin1, LOW); // Turn the motor off delay(refreshTime); } }
void MidRightOut() { for (int pulse=MidPulseS1; pulse >= MinPulseS1; pulse=pulse-rate){ digitalWrite(servoPin1, HIGH); // Turn the motor on delayMicroseconds(pulse); // Length of the pulse sets the motor position digitalWrite(servoPin1, LOW); // Turn the motor off delay(refreshTime); } }
void MidRightRet() { for (int pulse=MinPulseS1; pulse <= MidPulseS1; pulse=pulse+rate){ digitalWrite(servoPin1, HIGH); // Turn the motor on delayMicroseconds(pulse); // Length of the pulse sets the motor position digitalWrite(servoPin1, LOW); // Turn the motor off delay(refreshTime); } }
void StrideLeftOut() { for (int pulse=MidPulseS23+(rate/2); pulse <= MaxPulseS23; pulse=pulse+rate){ digitalWrite(servoPin2, HIGH); // Turn the motor on digitalWrite(servoPin3, HIGH); // Turn the motor on delayMicroseconds(pulse); // Length of the pulse sets the motor position digitalWrite(servoPin2, LOW); // Turn the motor off digitalWrite(servoPin3, LOW); // Turn the motor off delay(refreshTime); } }
void StrideLeftRet() { for (int pulse=MaxPulseS23; pulse >= MidPulseS23+(rate/2); pulse=pulse-rate){ digitalWrite(servoPin2, HIGH); // Turn the motor on digitalWrite(servoPin3, HIGH); // Turn the motor on delayMicroseconds(pulse); // Length of the pulse sets the motor position digitalWrite(servoPin2, LOW); // Turn the motor off digitalWrite(servoPin3, LOW); // Turn the motor off delay(refreshTime); } }
void StrideRightOut() { for (int pulse=MidPulseS23-(rate/2); pulse >= MinPulseS23; pulse=pulse-rate){ digitalWrite(servoPin2, HIGH); // Turn the motor on digitalWrite(servoPin3, HIGH); // Turn the motor on delayMicroseconds(pulse); // Length of the pulse sets the motor position digitalWrite(servoPin2, LOW); // Turn the motor off digitalWrite(servoPin3, LOW); // Turn the motor off delay(refreshTime); } }
void StrideRightRet() { for (int pulse=MinPulseS23; pulse <= MidPulseS23-(rate/2); pulse=pulse+rate){ digitalWrite(servoPin2, HIGH); // Turn the motor on digitalWrite(servoPin3, HIGH); // Turn the motor on delayMicroseconds(pulse); // Length of the pulse sets the motor position digitalWrite(servoPin2, LOW); // Turn the motor off digitalWrite(servoPin3, LOW); // Turn the motor off delay(refreshTime); } }
void TurnForwardOut() { for (int pulse=MidPulseS23-(rate/2); pulse >= MinPulseS23; pulse=pulse-rate){ digitalWrite(servoPin2, HIGH); // Turn the motor on digitalWrite(servoPin3, HIGH); // Turn the motor on delayMicroseconds(pulse); // Length of the pulse sets the motor position digitalWrite(servoPin2, LOW); // Turn the motor off delayMicroseconds(2800-pulse-pulse); // Length of the pulse sets the motor position digitalWrite(servoPin3, LOW); // Turn the motor off delay(refreshTime); } }
void TurnForwardRet() { for (int pulse=MinPulseS23; pulse <= MidPulseS23-(rate/2); pulse=pulse+rate){ digitalWrite(servoPin2, HIGH); // Turn the motor on digitalWrite(servoPin3, HIGH); // Turn the motor on delayMicroseconds(pulse); // Length of the pulse sets the motor position digitalWrite(servoPin2, LOW); // Turn the motor off delayMicroseconds(2800-pulse-pulse); // Length of the pulse sets the motor position digitalWrite(servoPin3, LOW); // Turn the motor off delay(refreshTime); } }
void TurnBackOut() { for (int pulse=MidPulseS23-(rate/2); pulse >= MinPulseS23; pulse=pulse-rate){ digitalWrite(servoPin3, HIGH); // Turn the motor on digitalWrite(servoPin2, HIGH); // Turn the motor on delayMicroseconds(pulse); // Length of the pulse sets the motor position digitalWrite(servoPin3, LOW); // Turn the motor off delayMicroseconds(2800-pulse-pulse); // Length of the pulse sets the motor position digitalWrite(servoPin2, LOW); // Turn the motor off delay(refreshTime); } }
void TurnBackRet() { for (int pulse=MinPulseS23; pulse <= MidPulseS23-(rate/2); pulse=pulse+rate){ digitalWrite(servoPin3, HIGH); // Turn the motor on digitalWrite(servoPin2, HIGH); // Turn the motor on delayMicroseconds(pulse); // Length of the pulse sets the motor position digitalWrite(servoPin3, LOW); // Turn the motor off delayMicroseconds(2800-pulse-pulse); // Length of the pulse sets the motor position digitalWrite(servoPin2, LOW); // Turn the motor off delay(refreshTime); } }
void Init() { digitalWrite(LeftLED, HIGH); // Turn on left led digitalWrite(RightLED, LOW); // Turn off right led MidPark(); digitalWrite(LeftLED, LOW); // Turn off left led digitalWrite(RightLED, HIGH); // Turn on right led MidLeftOut(); digitalWrite(LeftLED, HIGH); // Turn on left led digitalWrite(RightLED, LOW); // Turn off right led LeftPark(); digitalWrite(LeftLED, LOW); // Turn off left led digitalWrite(RightLED, HIGH); // Turn on right led MidLeftRet(); digitalWrite(LeftLED, HIGH); // Turn on left led digitalWrite(RightLED, LOW); // Turn off right led MidRightOut(); digitalWrite(LeftLED, LOW); // Turn off left led digitalWrite(RightLED, HIGH); // Turn on right led RightPark(); digitalWrite(LeftLED, HIGH); // Turn on left led digitalWrite(RightLED, LOW); // Turn off right led MidRightRet(); }
void Forward() { digitalWrite(LeftLED, HIGH); // Turn on left led digitalWrite(RightLED, HIGH); // Turn on right led StrideLeftOut(); MidRightRet(); MidLeftOut(); StrideLeftRet(); StrideRightOut(); MidLeftRet(); MidRightOut(); StrideRightRet(); }
void Back() { StrideLeftOut(); MidLeftRet(); digitalWrite(LeftLED, LOW); // Turn on left led digitalWrite(RightLED, LOW); // Turn on right led MidRightOut(); StrideLeftRet(); StrideRightOut(); MidRightRet(); digitalWrite(LeftLED, HIGH); // Turn on left led digitalWrite(RightLED, HIGH); // Turn on right led MidLeftOut(); StrideRightRet(); }
void TurnRight() { TurnBackOut(); MidLeftRet(); digitalWrite(LeftLED, LOW); // Turn on left led digitalWrite(RightLED, LOW); // Turn on right led MidRightOut(); TurnBackRet(); TurnForwardOut(); MidRightRet(); digitalWrite(RightLED, HIGH); // Turn on right led MidLeftOut(); TurnForwardRet(); }
void TurnLeft() { TurnForwardOut(); MidLeftRet(); digitalWrite(LeftLED, LOW); // Turn on left led digitalWrite(RightLED, LOW); // Turn on right led MidRightOut(); TurnForwardRet(); TurnBackOut(); MidRightRet(); digitalWrite(LeftLED, HIGH); // Turn on left led MidLeftOut(); TurnBackRet(); }
void setup() {
pinMode(servoPin1, OUTPUT); // Set servo pin 1 as an output pin for the middle legs pinMode(servoPin2, OUTPUT); // Set servo pin 2 as an output pin for the back right leg pinMode(servoPin3, OUTPUT); // Set servo pin 3 as an output pin for the back left leg
pinMode(LeftFeeler, INPUT); // Left Feeler pinMode(RightFeeler, INPUT); // Right Feeler
// LeftFeeler = HIGH ; // RightFeeler = HIGH ;
pinMode(LeftLED, OUTPUT); // Left LED pinMode(RightLED, OUTPUT); // Right LED
// Servo Callibration Mode is activated if both front feelers are activated on boot-up. if (digitalRead (LeftFeeler) == LOW) { if (digitalRead (RightFeeler) == LOW) { do { LeftPark(); digitalWrite(LeftLED, LOW); // Turn off left led digitalWrite(RightLED, HIGH); // Turn on right led RightPark(); digitalWrite(LeftLED, HIGH); // Turn on left led digitalWrite(RightLED, LOW); // Turn off right led MidPark(); } while (millis() < 100000); //Setup Mode Mode timeout in 100 seconds
}
} Init(); //Set roboot legs to start position
delay(3000); // wait 3 seconds.
}
// ************************************************************ // ******************** Atlas Hexapod Loop ******************** // ************************************************************
void loop() {
MidRightOut();
do { Forward(); Feeler=0; if (digitalRead (LeftFeeler) == LOW) (Feeler=1) ; if (digitalRead (RightFeeler) == LOW) (Feeler=(Feeler + 2)) ; } while (Feeler == 0) ;
MidRightRet(); MidLeftOut();
if (Feeler == 3) {
Back(); Back(); Back(); Back(); Back(); TurnLeft(); TurnLeft(); TurnLeft(); TurnLeft(); TurnLeft(); }
if (Feeler == 2) {
Back(); Back(); Back(); Back(); TurnLeft(); TurnLeft(); TurnLeft();
}
if (Feeler == 1) {
Back(); Back(); Back(); Back(); TurnRight(); TurnRight(); TurnRight();
}
MidLeftRet();
}
|