ch04

walkerForwardComplete.pde

// walkerForwardComplete.pde - Two servo walker. 
// Complete code with obstacle avoidance
// (c) Kimmo Karvinen & Tero Karvinen http://BotBook.com
// Updated by Joe Saavedra, 2010
#include <Servo.h> 
 
Servo frontServo;  
Servo rearServo;  
/* Servo motors - global variables */
int centerPos = 90; 
int frontRightUp = 72;
int frontLeftUp = 108;
int backRightForward = 75;
int backLeftForward = 105;
int walkSpeed = 150; // How long to wait between steps in milliseconds
int centerTurnPos = 81; 
int frontTurnRightUp = 63;
int frontTurnLeftUp = 117;
int backTurnRightForward = 66;
int backTurnLeftForward = 96;

/* Ping distance measurement - global variables */
int pingPin = 4;
long int duration, distanceInches;
long distanceFront=0; //cm
int startAvoidanceDistance=20; //cm

long microsecondsToInches(long microseconds)
{
  return microseconds / 74 / 2;
}

long microsecondsToCentimeters(long microseconds)
{
  return microseconds / 29 / 2;
}

long distanceCm(){
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);

  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);

  distanceInches = microsecondsToInches(duration);
  return microsecondsToCentimeters(duration);
}

void center()
{
  frontServo.write(centerPos);
  rearServo.write(centerPos);  
}

void moveForward()
{
  frontServo.write(frontRightUp);
  rearServo.write(backLeftForward);
  delay(125);
  frontServo.write(centerPos);
  rearServo.write(centerPos);
  delay(65);
  frontServo.write(frontLeftUp);
  rearServo.write(backRightForward);
  delay(125);
  
  frontServo.write(centerPos);
  rearServo.write(centerPos);
  delay(65);
}

void moveBackRight()
{
  frontServo.write(frontRightUp);
  rearServo.write(backRightForward-6);
  delay(125);
  frontServo.write(centerPos);
  rearServo.write(centerPos-6);
  delay(65);
  frontServo.write(frontLeftUp+9);
  rearServo.write(backLeftForward-6);
  delay(125);
  
  frontServo.write(centerPos);
  rearServo.write(centerPos);
  delay(65);
}

void moveTurnLeft()
{
  frontServo.write(frontTurnRightUp);
  rearServo.write(backTurnLeftForward);
  delay(125);
  frontServo.write(centerTurnPos);
  rearServo.write(centerTurnPos);
  delay(65);
  frontServo.write(frontTurnLeftUp);
  rearServo.write(backTurnRightForward);
  delay(125);
  
  frontServo.write(centerTurnPos);
  rearServo.write(centerTurnPos);
  delay(65);
}

void setup()
{
  frontServo.attach(2);
  rearServo.attach(3);
  pinMode(pingPin, OUTPUT);
}  

void loop()
{
  distanceFront=distanceCm();
  if (distanceFront > 1){ // Filters out any stray 0.00 error readings
    if (distanceFront<startAvoidanceDistance) {
      for(int i=0; i<=8; i++) {
        moveBackRight();
        delay(walkSpeed);
      }
      for(int i=0; i<=10; i++) {
        moveTurnLeft();
        delay(walkSpeed);
      }
    } else {
      moveForward(); bk
      delay(walkSpeed);
    }
  }
}

walkerForward.pde

// walkerForward.pde - Two servo walker. Forward.
// (c) Kimmo Karvinen & Tero Karvinen http://BotBook.com
// updated - Joe Saavedra, 2010
#include <Servo.h> 
 
Servo frontServo;  
Servo rearServo;  
int centerPos = 90;
int frontRightUp = 72;
int frontLeftUp = 108;
int backRightForward = 75;
int backLeftForward = 105;
void moveForward()
{
    frontServo.write(frontRightUp);
  rearServo.write(backLeftForward);
  delay(125);
  frontServo.write(centerPos);
  rearServo.write(centerPos);
  delay(65);
  frontServo.write(frontLeftUp);
  rearServo.write(backRightForward);
  delay(125);
  
  frontServo.write(centerPos);
  rearServo.write(centerPos);
  delay(65);
}
 
void setup()
{
  frontServo.attach(2);
  rearServo.attach(3);
}

void loop()
{
  moveForward();
  delay(150);  //time between each step taken, speed of walk
}

walkerTurnBackward.pde

// walkerTurnBackward.pde - Two servo walker. Turn backward.
// (c) Kimmo Karvinen & Tero Karvinen http://BotBook.com
// updated - Joe Saavedra, 2010
#include <Servo.h> 
 
Servo frontServo;  
Servo rearServo;  
int centerPos = 90; 
int frontRightUp = 72;
int frontLeftUp = 108;
int backRightForward = 75;
int backLeftForward = 105;

void moveBackRight()
{
  frontServo.write(frontRightUp);
  rearServo.write(backRightForward-6);
  delay(125);
  frontServo.write(centerPos);
  rearServo.write(centerPos-6);
  delay(65);
  frontServo.write(frontLeftUp+9);
  rearServo.write(backLeftForward-6);
  delay(125);
  frontServo.write(centerPos);
  rearServo.write(centerPos);
  delay(65);
}
 
void setup()
{
  frontServo.attach(2);
  rearServo.attach(3);
}

void loop()
{
  moveBackRight();
  delay(150);  //time between each step taken, speed of walk
}

walkerTurnForward.pde

// walkerTurnForward.pde - Two servo walker. Turn forward.
// (c) Kimmo Karvinen & Tero Karvinen http://BotBook.com
// updated - Joe Saavedra, 2010
#include <Servo.h> 
 
Servo frontServo;  
Servo rearServo;  
int centerTurnPos = 81;
int frontTurnRightUp = 63;
int frontTurnLeftUp = 117;
int backTurnRightForward = 66;
int backTurnLeftForward = 96;

void moveTurnLeft()
{
  frontServo.write(frontTurnRightUp);
  rearServo.write(backTurnLeftForward);
  delay(125);
  frontServo.write(centerTurnPos);
  rearServo.write(centerTurnPos);
  delay(65);
  frontServo.write(frontTurnLeftUp);
  rearServo.write(backTurnRightForward);
  delay(125);
  frontServo.write(centerTurnPos);
  rearServo.write(centerTurnPos);
  delay(65);
}
 
void setup()
{
  frontServo.attach(2);
  rearServo.attach(3);
}

void loop()
{
  moveTurnLeft();
  delay(150);  //time between each step taken, speed of walk
}

stalkerguard.pde

// servoCenter.pde - Center servo
// (c) Kimmo Karvinen & Tero Karvinen http://BotBook.com
// updated – Joe Saavedra, 2010
#include <Servo.h>
 
Servo myServo;
 
void setup() 
{ 
  myServo.attach(2);
  myServo.write(90);
} 
 
void loop() 
{ 
  delay(100); 
} 

moveServo.pde

// moveServo.pde - Move servo to center, maximum angle
// and to minumum angle
// (c) Kimmo Karvinen & Tero Karvinen http://BotBook.com
// updated - Joe Saavedra, 2010
#include <Servo.h> 
 
Servo myServo;  
int delayTime = 1000;

void setup() 
{ 
  myServo.attach(2);   
} 
 
void loop() 
{ 
  myServo.write(90);
  delay(delayTime);
  
  myServo.write(180);
  delay(delayTime);
  
  myServo.write(90);
  delay(delayTime);
  myServo.write(0);
  delay(delayTime);
} 

twoServosCenter.pde

//twoServosCenter.pde - Center two servos
// (c) Kimmo Karvinen & Tero Karvinen http://BotBook.com
//updated - Joe Saavedra, 2010
#include <Servo.h> 
 
Servo frontServo;  
Servo rearServo;

void setup() 
{ 
  frontServo.attach(2); 
  rearServo.attach(3);
  frontServo.write(90);
  rearServo.write(90);
} 
 
void loop() 
{ 
    delay(100);
}

walkerBackward.pde

// walkerBackward.pde - Two servo walker. Backward.
// (c) Kimmo Karvinen & Tero Karvinen http://BotBook.com
// updated - Joe Saavedra, 2010
#include <Servo.h> 

Servo frontServo;  
Servo rearServo;  
int centerPos = 90; 
int frontRightUp = 72;
int frontLeftUp = 108;
int backRightForward = 75;
int backLeftForward = 105;

void moveBackward()
{
  frontServo.write(frontRightUp);
  rearServo.write(backRightForward);
  delay(125);
  frontServo.write(centerPos);
  rearServo.write(centerPos);
  delay(65);
  frontServo.write(frontLeftUp);
  rearServo.write(backLeftForward);
  delay(125);
  frontServo.write(centerPos);
  rearServo.write(centerPos);
  delay(65);
}
 
void setup()
{
  frontServo.attach(2);
  rearServo.attach(3);
}

void loop()
{
  moveBackward();
  delay(150);  //time between each step taken, speed of walk
}