*Office Supplies HEXAPOD Code
- include <Servo.h>
- define MAX_DEGREE 145
- define MIN_DEGREE 35
- define MID_DEGREE 90
- define BASIC_DELAY 250
Servo FL; // create servo object to control a servo Servo FR; Servo ML; Servo MR; Servo RL; Servo RR;
void setup()
{
// put your setup code here, to run once: FL.attach(3); FR.attach(5); ML.attach(6); MR.attach(9); RL.attach(10); RR.attach(11);
}
void Stand() {
FL.write(MID_DEGREE); FR.write(MID_DEGREE); ML.write(MID_DEGREE); MR.write(MID_DEGREE); RL.write(MID_DEGREE); RR.write(MID_DEGREE);
}
void Left_Position_Forward() {
FL.write(MIN_DEGREE); RL.write(MIN_DEGREE);
}
void Left_Position_Backward() {
FL.write(MAX_DEGREE); RL.write(MAX_DEGREE);
}
void Left_Position_Outward() {
FL.write(MIN_DEGREE); RL.write(MAX_DEGREE);
}
void Left_Position_Inward() {
FL.write(MAX_DEGREE); RL.write(MIN_DEGREE);
}
void Left_Position_Stand() {
FL.write(MID_DEGREE); RL.write(MID_DEGREE);
}
void Right_Position_Forward() {
FR.write(MAX_DEGREE); RR.write(MAX_DEGREE);
}
void Right_Position_Backward()
{
FR.write(MIN_DEGREE); RR.write(MIN_DEGREE);
}
void Right_Position_Outward() {
FR.write(MAX_DEGREE); RR.write(MIN_DEGREE);
}
void Right_Position_Inward() {
FR.write(MIN_DEGREE); RR.write(MAX_DEGREE);
}
void Right_Position_Stand() {
FR.write(MID_DEGREE); RR.write(MID_DEGREE);
}
void Middle_Position_Left() {
ML.write(MAX_DEGREE); MR.write(MAX_DEGREE);
}
void Middle_Position_Right() {
ML.write(MIN_DEGREE); MR.write(MIN_DEGREE);
}
void Middle_Position_Stand() {
ML.write(MID_DEGREE); MR.write(MID_DEGREE);
}
void Middle_Position_Outward() {
ML.write(MAX_DEGREE); MR.write(MIN_DEGREE);
}
void Middle_Position_Inward() {
ML.write(MIN_DEGREE); MR.write(MAX_DEGREE);
}
void Middle_Lean_Left() {
ML.write(MIN_DEGREE); MR.write(MID_DEGREE); Left_Position_Outward();
}
void Middle_Lean_Right() {
ML.write(MID_DEGREE); MR.write(MAX_DEGREE); Right_Position_Outward();
}
void Step_Forward() {
Stand(); delay(BASIC_DELAY); Left_Position_Forward(); delay(BASIC_DELAY); Middle_Position_Inward(); delay(BASIC_DELAY); Left_Position_Backward(); delay(BASIC_DELAY); Middle_Position_Stand(); delay(BASIC_DELAY); Left_Position_Stand(); delay(BASIC_DELAY); Right_Position_Forward(); delay(BASIC_DELAY); Middle_Position_Inward(); delay(BASIC_DELAY); Right_Position_Backward(); delay(BASIC_DELAY); Middle_Position_Stand(); delay(BASIC_DELAY); Right_Position_Stand(); delay(BASIC_DELAY);
}
void Step_Backward() {
Stand(); delay(BASIC_DELAY); Left_Position_Backward(); delay(BASIC_DELAY); Middle_Position_Inward(); delay(BASIC_DELAY); Left_Position_Forward(); delay(BASIC_DELAY); Middle_Position_Stand(); delay(BASIC_DELAY); Left_Position_Stand(); delay(BASIC_DELAY); Right_Position_Backward(); delay(BASIC_DELAY); Middle_Position_Inward(); delay(BASIC_DELAY); Right_Position_Forward(); delay(BASIC_DELAY); Middle_Position_Stand(); delay(BASIC_DELAY); Right_Position_Stand(); delay(BASIC_DELAY);
}
void Step_Left() {
Stand(); delay(BASIC_DELAY); Right_Position_Forward(); delay(BASIC_DELAY); Middle_Position_Inward(); delay(BASIC_DELAY); Right_Position_Backward(); delay(BASIC_DELAY); Middle_Position_Stand(); delay(BASIC_DELAY); Right_Position_Stand(); delay(BASIC_DELAY);
}
void Step_Right() {
Stand(); delay(BASIC_DELAY); Left_Position_Forward(); delay(BASIC_DELAY); Middle_Position_Inward(); delay(BASIC_DELAY); Left_Position_Backward(); delay(BASIC_DELAY); Middle_Position_Stand(); delay(BASIC_DELAY); Left_Position_Stand(); delay(BASIC_DELAY);
}
int Left_Sensor; int Right_Sensor; int Front_Sensor; int Rear_Sensor;
void Set_Sensors() {
Left_Sensor = analogRead(2); Right_Sensor = analogRead(1); Front_Sensor = analogRead(3); Rear_Sensor = analogRead(0);
}
int Sensor_Approximation(int S, int V) {
if (V > S * 1.2) return (1); if (V < S * 0.8) return(0); return(0);
}
int Dark_Direction() {
}
void loop()
{
int x;
Stand();
delay(2000);
Set_Sensors();
while(1)
{
if (Sensor_Approximation(Left_Sensor, analogRead(2)))
{
for(x=0;x<5;++x)
Step_Right();
Set_Sensors();
}
if (Sensor_Approximation(Right_Sensor, analogRead(1)))
{
for(x=0;x<5;++x)
Step_Left();
Set_Sensors();
}
if (Sensor_Approximation(Front_Sensor, analogRead(3)))
{
for(x=0;x<5;++x)
Step_Backward();
Set_Sensors();
}
if (Sensor_Approximation(Rear_Sensor, analogRead(0)))
{
for(x=0;x<5;++x)
Step_Forward();
Set_Sensors();
}
}
delay(2000);
// put your main code here, to run repeatedly:
}