/* Autonomous Quadruped */ #include Servo panServo; // Define servo objects for the four ankle joints Servo ankleFL; Servo ankleFR; Servo ankleBL; Servo ankleBR; // Define servo objects for the four hip joints Servo hipFL; Servo hipFR; Servo hipBL; Servo hipBR; // Define variables int flex1 = 45; // Determines the length of each stride int delayTime = 500; // Delay between limb movements int startPause = 5000; // Delay to allow robot placement before movement int pos = 0; // Loop counter int smoothnessDelay = 15; int flexLeft = 45; // Length of left stride int flexRight = 15; // Make right stride one half or one third of left stride to turn right int multiplier = 3; // Make multiplier 2 if right is one half of left // stride and make multiplier 3 if right is one third // Declare digital pins int servoPinPan = 3; // Declare analog pins int IRpin = 4; // Define variables int distanceReading; int wallDistance; int wallDistanceTolerance = 30; int distanceReadingLeft; int distanceReadingRight; int wallDistanceLeft; int wallDistanceRight; int panDelay = 1000; // Delay to allow IR sensor to take a reading int reverseTime = 3; int reverseTimeCounter = 0; int turnTime = 5; int turnTimeCounter=0; int pauseTime = 1000; void setup() { panServo.attach(servoPinPan); hipFL.attach(10); ankleFL.attach(11); hipFR.attach(4); ankleFR.attach(5); hipBL.attach(6); ankleBL.attach(7); hipBR.attach(8); ankleBR.attach(9); // Put legs in starting position with // front left and back right legs in forward position and // front right and back left legs in rearward position hipFL.write(90-flex1); ankleFL.write(90-flex1); hipFR.write(90+flex1); ankleFR.write(90+flex1); hipBL.write(90+flex1); ankleBL.write(90+flex1); hipBR.write(90-flex1); ankleBR.write(90-flex1); delay(startPause); // Uncomment the serial feed for testing if need be Serial.begin(9600); } void loop() { // Point distance sensor straight ahead panServo.write(90); // Move forward // Straighten front left and back right legs delay(delayTime); for(pos = 0; pos < flex1; pos += 1) { hipFL.write(90-flex1+pos); ankleFL.write(90-flex1+pos); hipBR.write(90-flex1+pos); ankleBR.write(90-flex1+pos); delay(smoothnessDelay); } // Bring front right and back left legs forward in three steps // First flex ankles forward delay(delayTime); ankleFR.write(180); ankleBL.write(180); // Second, swing the front right and back left legs forward delay(delayTime); hipFR.write(90-flex1); hipBL.write(90-flex1); // Third, reset front right and back left ankles delay(delayTime); ankleFR.write(90-flex1); ankleBL.write(90-flex1); // Bring front left and back right legs rearward delay(delayTime); for(pos = 0; pos < flex1; pos += 1) { hipFL.write(90+pos); ankleFL.write(90+pos); hipBR.write(90+pos); ankleBR.write(90+pos); delay(smoothnessDelay); } // Straighten front right and back left legs delay(delayTime); for(pos = 0; pos < flex1; pos += 1) { hipFR.write(90-flex1+pos); ankleFR.write(90-flex1+pos); hipBL.write(90-flex1+pos); ankleBL.write(90-flex1+pos); delay(smoothnessDelay); } // Bring front left and back right legs forward in three steps // First flex ankles forward delay(delayTime); // ankleFL.write(180); hipFL.write(180); ankleBR.write(180); // Second, swing the front left and back right legs forward delay(delayTime); // hipFL.write(90-flex1); ankleFL.write(90-flex1); hipBR.write(90-flex1); // Third, reset front left and back right ankles delay(delayTime); // ankleFL.write(90-flex1); hipFL.write(90-flex1); ankleBR.write(90-flex1); // Bring front right and back left legs rearward delay(delayTime); for(pos = 0; pos < flex1; pos += 1) { hipFR.write(90+pos); ankleFR.write(90+pos); hipBL.write(90+pos); ankleBL.write(90+pos); delay(smoothnessDelay); } // Take reading from distance sensor distanceReading = analogRead(IRpin); wallDistance = 40-distanceReading/10; // The wall distance formula above is determined by trial // and error and linear conversion // Test to see if a wall is near if (wallDistance < wallDistanceTolerance) { // Stop hipFL.write(90-flex1); ankleFL.write(90-flex1); hipFR.write(90+flex1); ankleFR.write(90+flex1); hipBL.write(90+flex1); ankleBL.write(90+flex1); hipBR.write(90-flex1); ankleBR.write(90-flex1); // Pan distance servo left and right to see which direction // offers a clearer path panServo.write(170); delay(panDelay); distanceReadingLeft = analogRead(IRpin); delay(panDelay); wallDistanceLeft = 40-distanceReadingLeft/10; panServo.write(20); delay(panDelay); distanceReadingRight = analogRead(IRpin); delay(panDelay); wallDistanceRight = 40-distanceReadingRight/10; panServo.write(90); delay(panDelay); // Uncomment serial print statements for debugging purposes Serial.print(wallDistance); Serial.println(" cm"); Serial.print(wallDistanceLeft); Serial.println(" cm"); Serial.print(wallDistanceRight); Serial.println(" cm"); Serial.println(" "); Serial.println(distanceReading); Serial.println(distanceReadingLeft); Serial.println(distanceReadingRight); Serial.println(" "); // Test to see which direction offers a clear path // and turn the robot in that direction if (wallDistanceLeft > wallDistanceRight) { // Reverse for (reverseTimeCounter = 0; reverseTimeCounter < reverseTime; reverseTimeCounter += 1) { // Straighten front right and back left legs delay(delayTime); for(pos = 0; pos < flex1; pos += 1) { hipFR.write(90+flex1-pos); ankleFR.write(90+flex1-pos); hipBL.write(90+flex1-pos); ankleBL.write(90+flex1-pos); delay(smoothnessDelay); } // Bring front left and back right legs rearward in three steps // First flex ankles rearward delay(delayTime); // ankleFL.write(0); hipFL.write(0); ankleBR.write(0); // Second, swing the front right and back left legs rearward delay(delayTime); // hipFL.write(90+flex1); ankleFL.write(90+flex1); hipBR.write(90+flex1); // Third, reset front right and back left ankles delay(delayTime); // ankleFL.write(90+flex1); hipFL.write(90+flex1); ankleBR.write(90+flex1); // Bring the front right and back left legs forward delay(delayTime); for(pos = 0; pos < flex1; pos += 1) { hipFR.write(90-pos); ankleFR.write(90-pos); hipBL.write(90-pos); ankleBL.write(90-pos); delay(smoothnessDelay); } // Straighten the front left and back right legs delay(delayTime); for(pos = 0; pos < flex1; pos += 1) { hipFL.write(90+flex1-pos); ankleFL.write(90+flex1-pos); hipBR.write(90+flex1-pos); ankleBR.write(90+flex1-pos); delay(smoothnessDelay); } // Bring front right and back left legs rearward in three steps // First flex ankles rearward delay(delayTime); ankleFR.write(0); ankleBL.write(0); // Second, swing the front right and back left legs rearward delay(delayTime); hipFR.write(90+flex1); hipBL.write(90+flex1); // Third, reset front right and back left ankles delay(delayTime); ankleFR.write(90+flex1); ankleBL.write(90+flex1); // Bring front left and back right legs forward delay(delayTime); for(pos = 0; pos < flex1; pos += 1) { hipFL.write(90-pos); ankleFL.write(90-pos); hipBR.write(90-pos); ankleBR.write(90-pos); delay(smoothnessDelay); } } // Turn left for (turnTimeCounter = 0; turnTimeCounter < turnTime; turnTimeCounter += 1) { // Straighten front left and back right legs // I use one loop to keep the limbs sychronized // Note the right limb must travel twice as far // as the left limb during a left turn, thus the // factor of 2*pos for the right limb loop counter delay(delayTime); for(pos = 0; pos < flexLeft; pos += 1) { hipFL.write(90-flexLeft+pos); ankleFL.write(90-flexLeft+pos); hipBR.write(90-flexRight+multiplier*pos); ankleBR.write(90-flexRight+multiplier*pos); delay(smoothnessDelay); } // Bring front right and back left legs forward in three steps // First flex ankles forward delay(delayTime); ankleFR.write(180); ankleBL.write(180); // Second, swing the front right and back left legs forward delay(delayTime); hipFR.write(90-flexRight); hipBL.write(90-flexLeft); // Third, reset front right and back left ankles delay(delayTime); ankleFR.write(90-flexRight); ankleBL.write(90-flexLeft); // Bring front left and back right legs rearward delay(delayTime); for(pos = 0; pos < flexLeft; pos += 1) { hipFL.write(90+pos); ankleFL.write(90+pos); hipBR.write(90+multiplier*pos); ankleBR.write(90+multiplier*pos); delay(smoothnessDelay); } // Straighten front right and back left legs delay(delayTime); for(pos = 0; pos < flexLeft; pos += 1) { hipFR.write(90-flexRight+multiplier*pos); ankleFR.write(90-flexRight+multiplier*pos); hipBL.write(90-flexLeft+pos); ankleBL.write(90-flexLeft+pos); delay(smoothnessDelay); } // Bring front left and back right legs forward in three steps // First flex ankles forward delay(delayTime); // ankleFL.write(180); hipFL.write(180); ankleBR.write(180); // Second, swing the front left and back right legs forward delay(delayTime); // hipFL.write(90-flex1); ankleFL.write(90-flexLeft); hipBR.write(90-flexRight); // Third, reset front left and back right ankles delay(delayTime); // ankleFL.write(90-flex1); hipFL.write(90-flexLeft); ankleBR.write(90-flexRight); // Bring front right and back left legs rearward delay(delayTime); for(pos = 0; pos < flexLeft; pos += 1) { hipFR.write(90+multiplier*pos); ankleFR.write(90+multiplier*pos); hipBL.write(90+pos); ankleBL.write(90+pos); delay(smoothnessDelay); } } } else { // Reverse for (reverseTimeCounter = 0; reverseTimeCounter < reverseTime; reverseTimeCounter += 1) { // Straighten front right and back left legs delay(delayTime); for(pos = 0; pos < flex1; pos += 1) { hipFR.write(90+flex1-pos); ankleFR.write(90+flex1-pos); hipBL.write(90+flex1-pos); ankleBL.write(90+flex1-pos); delay(smoothnessDelay); } // Bring front left and back right legs rearward in three steps // First flex ankles rearward delay(delayTime); // ankleFL.write(0); hipFL.write(0); ankleBR.write(0); // Second, swing the front right and back left legs rearward delay(delayTime); // hipFL.write(90+flex1); ankleFL.write(90+flex1); hipBR.write(90+flex1); // Third, reset front right and back left ankles delay(delayTime); // ankleFL.write(90+flex1); hipFL.write(90+flex1); ankleBR.write(90+flex1); // Bring the front right and back left legs forward delay(delayTime); for(pos = 0; pos < flex1; pos += 1) { hipFR.write(90-pos); ankleFR.write(90-pos); hipBL.write(90-pos); ankleBL.write(90-pos); delay(smoothnessDelay); } // Straighten the front left and back right legs delay(delayTime); for(pos = 0; pos < flex1; pos += 1) { hipFL.write(90+flex1-pos); ankleFL.write(90+flex1-pos); hipBR.write(90+flex1-pos); ankleBR.write(90+flex1-pos); delay(smoothnessDelay); } // Bring front right and back left legs rearward in three steps // First flex ankles rearward delay(delayTime); ankleFR.write(0); ankleBL.write(0); // Second, swing the front right and back left legs rearward delay(delayTime); hipFR.write(90+flex1); hipBL.write(90+flex1); // Third, reset front right and back left ankles delay(delayTime); ankleFR.write(90+flex1); ankleBL.write(90+flex1); // Bring front left and back right legs forward delay(delayTime); for(pos = 0; pos < flex1; pos += 1) { hipFL.write(90-pos); ankleFL.write(90-pos); hipBR.write(90-pos); ankleBR.write(90-pos); delay(smoothnessDelay); } } // Turn right for (turnTimeCounter = 0; turnTimeCounter < turnTime; turnTimeCounter += 1) { // Straighten front left and back right legs // I use one loop to keep the limbs sychronized // Note the right limb must travel twice as far // as the left limb during a left turn, thus the // factor of 2*pos for the right limb loop counter delay(delayTime); for(pos = 0; pos < flexRight; pos += 1) { hipFL.write(90-flexLeft+multiplier*pos); ankleFL.write(90-flexLeft+multiplier*pos); hipBR.write(90-flexRight+pos); ankleBR.write(90-flexRight+pos); delay(smoothnessDelay); } // Bring front right and back left legs forward in three steps // First flex ankles forward delay(delayTime); ankleFR.write(180); ankleBL.write(180); // Second, swing the front right and back left legs forward delay(delayTime); hipFR.write(90-flexRight); hipBL.write(90-flexLeft); // Third, reset front right and back left ankles delay(delayTime); ankleFR.write(90-flexRight); ankleBL.write(90-flexLeft); // Bring front left and back right legs rearward delay(delayTime); for(pos = 0; pos < flexRight; pos += 1) { hipFL.write(90+multiplier*pos); ankleFL.write(90+multiplier*pos); hipBR.write(90+pos); ankleBR.write(90+pos); delay(smoothnessDelay); } // Straighten front right and back left legs delay(delayTime); for(pos = 0; pos < flexRight; pos += 1) { hipFR.write(90-flexRight+pos); ankleFR.write(90-flexRight+pos); hipBL.write(90-flexLeft+multiplier*pos); ankleBL.write(90-flexLeft+multiplier*pos); delay(smoothnessDelay); } // Bring front left and back right legs forward in three steps // First flex ankles forward delay(delayTime); // ankleFL.write(180); hipFL.write(180); ankleBR.write(180); // Second, swing the front left and back right legs forward delay(delayTime); // hipFL.write(90-flex1); ankleFL.write(90-flexLeft); hipBR.write(90-flexRight); // Third, reset front left and back right ankles delay(delayTime); // ankleFL.write(90-flex1); hipFL.write(90-flexLeft); ankleBR.write(90-flexRight); // Bring front right and back left legs rearward delay(delayTime); for(pos = 0; pos < flexRight ; pos += 1) { hipFR.write(90+pos); ankleFR.write(90+pos); hipBL.write(90+multiplier*pos); ankleBL.write(90+multiplier*pos); delay(smoothnessDelay); } } } } }