ATtiny84 3-servo hexapod walking robot

[tylernt]

http://www.youtube.com/watch?v=E58JDq0wDrw

Similar to http://www.pololu.com/docs/0J42/all only with my own perfboard circuit and sensor design.

Code: [Select]


// http://code.google.com/p/arduino-tiny/ pinout
//                      +-\/-+
//                VCC  1|    |14  GND
//                  0  2|    |13  10/A0
//                  1  3|    |12  9/A1
//              RESET  4|    |11  8/A2
//                  2  5|    |10  7/A3
//               3/A7  6|    |9   6/A4
//               4/A6  7|    |8   5/A5
//                      +----+

#define SERVOS 3 // set this to how many servos you have, 8 max
const byte servoPin[SERVOS] = { 10, 6, 5 }; // place your servo pin numbers here
#define RIGHTSERVO 0 // give servo array indexes nice names
#define LEFTSERVO 1
#define MIDDLESERVO 2
byte servoPos[SERVOS]; // don't mess with this. It stores the number of timer ticks for this servo's HIGH pulse
#define SERVOMIN 19 // the HIGH pulse will be this many timer ticks long to send the servo to 0°
#define SERVOMAX 75 // the HIGH pulse will be this many timer ticks long to send the servo to 180°
#define SERVOTIMESLICE (625/SERVOS) // don't mess with this. It's used to multiplex multiple servos into one timer
volatile byte currentServo = 0; // don't mess with this. It's used to track which servo gets it's HIGH pulse next

const byte leftServoCenter = 90; // neutral position
const byte rightServoCenter = 90; // neutral position
const byte middleServoCenter = 100; // neutral position
const byte sideStrideLength = 25; // how big of a step to take with front/rear legs (left/right servos). Offset either side from Center
const byte middleLiftHeight = 25; // how high to lift one side of our body to take a stride. Offset either side from Center
unsigned long strideTimer = 0; // schedule when to take a new stride
const byte liftInterval = 200; // it takes about n milliseconds to lift one side of our body
const byte strideInterval = 100; // it takes about n milliseconds to complete one subphase of a stride
const byte straightenStrides = 2; // how many full strides it takes to avoid an obstacle

// walking direction finite state machine states
#define STRAIGHT 0
#define STRAIGHTENLEFT 1
#define LEFT 2
#define STRAIGHTENRIGHT 3
#define RIGHT 4

const byte numSensors = 2; // how many photodiodes ('eyes') do we have?
const byte sensor[numSensors] = { A1, A3 }; // what pins are our photodiodes connected to
const byte sensorThreshold[numSensors] = { 10, 10 }; // each photodiode (and ADC mux!) has a slightly different sensitivity, we must adjust for this
byte currentSensor = 0; // global (yuck) variable keeps track of which eye we're looking through
const byte emitter = 0; // IR LED pin
unsigned long sensorTimer = 0; // schedule when to take a new sensor reading
const byte sensorInterval = 1; // take a sensor reading every n milliseconds

// can't over-discharge LiPo batteries!
const byte batteryMonitor = A2;
#define LOWBATTERY 850 // 1024 = 1.05V (*4 = 4.2V), 898 = .95V (*4 = 3.8V)
unsigned long batteryTimer = 0;

// ================================================================================================================================================
// translates a desired servo angle of 0 to 180° into the number of timer ticks the HIGH pulse should be
void moveServo(const byte servo, const byte pos)
{
servoPos[servo] = bmap(pos, 0, 180, SERVOMIN, SERVOMAX);
}

// ================================================================================================================================================
// a faster byte-based 'map' than the built-in function that uses slower 'long' arithmetic
byte bmap(const byte x, const byte in_min, const byte in_max, const byte out_min, const byte out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

// ================================================================================================================================================
void setupServos()
{
for(byte i=0; i < SERVOS; i++)
  {
  pinMode(servoPin[i], OUTPUT);
  moveServo(i, 90);
  }

noInterrupts(); // set up the hardware timer1
TCCR1A = 0; // timer1 Normal operation, OC1A/OC1B (the hardware PWM output pins) disconnected
OCR1A = 1; // Output Compare Register 1 A -- how many ticks before calling ISR
// each 50Hz servo needs a HIGH of 600uS to 2400uS, followed by a LOW of ~20,000uS
// timer tick = 32uS with 256 prescaler (31.25kHz @ 8MHz)
// this translates to a servo HIGH of 19 ticks to 75 ticks, with LOW of 625 ticks
// by the way, 625 ticks for a complete 1 second cycle allows up to 8 servos (625/75=8.3) to fit into one timer
// 19 to 75 ticks gives us a servo resolution of 56 steps. Pretty coarse, but for a walking robot we only need to jump
// from one extreme to another as fast as the servo will go. We could go to a prescaler of 64 if more resolution is needed,
// but then the timer ticks will need some recalculation and some variables have to go to 'int'
TCCR1B = (1 << WGM12) | (1 << CS12); // timer1 waveform WGM12=clear on timer compare. CS12=prescale of 256
TIMSK1 = (1 << OCIE1A);   // timer1 Output Compare A Match Interrupt (TIM1_COMPA) Enable
interrupts(); // all done!
}

// ================================================================================================================================================
ISR(TIM1_COMPA_vect) // timer compare interrupt
{
if(digitalRead(servoPin[currentServo]) == LOW) // servo has finished a long rest and now needs to go HIGH for a short pulse
  {
  digitalWrite(servoPin[currentServo], HIGH);
  OCR1A = servoPos[currentServo]; // schedule end of HIGH pulse
  }
else // servo has finished a short active pulse and now needs a long LOW rest
  {
  digitalWrite(servoPin[currentServo], LOW);
  OCR1A = SERVOTIMESLICE - servoPos[currentServo]; // schedule next servo
  currentServo++; // next servo's turn
  if(currentServo == SERVOS) // finished servicing all servos, so...
    { currentServo = 0; } // ...start over with the first
  }
return;
}

// ===================================================================================================================================
char readSensor()
{
static int sensorHit[numSensors] = { 0, 0 };
char goodSolidHit = -1;

int sensorAmbient = analogRead(sensor[currentSensor]);
digitalWrite(emitter,HIGH);
int sensorReflected = analogRead(sensor[currentSensor]);
digitalWrite(emitter,LOW);

int reading = sensorAmbient - sensorReflected;
if(reading > sensorThreshold[currentSensor]) // we've potentially detected an obstacle
  { sensorHit[currentSensor]++; }
else
  { sensorHit[currentSensor] = 0; } // hm nope nothing there

if(sensorHit[currentSensor] > 10) // we've detected an obstacle 10 times in a row, must be real
  { goodSolidHit = currentSensor; }

currentSensor++;
if(currentSensor > numSensors)
  { currentSensor = 0; }

return goodSolidHit;
}

// ===================================================================================================================================
void checkBattery()
{
analogReference(INTERNAL); // can't use VCC or AREF because those voltages fall along with the battery
pinMode(batteryMonitor, INPUT);
analogRead(batteryMonitor); // The first ADC conversion result after switching reference voltage source may be inaccurate, and the user is advised to discard this result.
if(analogRead(batteryMonitor) < LOWBATTERY) // LiPo dangerously low...
  { while(1); } // ...so stop walking / firing emitter
analogReference(DEFAULT); // go back to VCC so the photodiodes work right
analogRead(batteryMonitor); // The first ADC conversion result after switching reference voltage source may be inaccurate, and the user is advised to discard this result.
}


[tylernt]

Code: [Select]


// ===================================================================================================================================
void setup()
{
for(byte i=0; i < numSensors; i++)
  { pinMode(sensor[i], INPUT); }
pinMode(emitter, OUTPUT);
pinMode(1, INPUT_PULLUP); // help pull up reset
pinMode(2, INPUT_PULLUP); // help pull up reset
pinMode(3, INPUT_PULLUP); // help pull up reset

delay(4000);

setupServos(); // It's not necessary to set the servo pins as OUTPUT, that's taken care of for you
}

// ===================================================================================================================================
void loop()
{
static byte state = STRAIGHT;
static byte strideCounter = straightenStrides;

if( millis() - sensorTimer > sensorInterval ) // time to take a sensor reading
  {
  sensorTimer += sensorInterval; // schedule next sensor reading
  char sensorResult = readSensor();
  switch(state)
    {
    case STRAIGHT:
      if( sensorResult == 0 ) // we've detected an obstacle on the left
        { state = RIGHT; }
      else if( sensorResult == 1 ) // we've detected an obstacle on the right
        { state = LEFT; }
      break;

    case STRAIGHTENLEFT:
    case STRAIGHTENRIGHT:
      if( strideCounter < 1 ) // we've been turning for staightenStride strides, so...
        { state = STRAIGHT; } // ...we're done turning and can go straight now
      if( sensorResult > -1 ) // encountered a new obstacle while avoiding the last one
        { state++; } // push to full turn
      break;

    case LEFT:
    case RIGHT:
      strideCounter = straightenStrides;
      if (sensorResult < 0) // we've cleared the obstacle
        { state--; } // fall back to straighten
      break;
    }
  }

if( millis() > batteryTimer ) // time to check the battery
  {
  batteryTimer += 1000;
  checkBattery();
  }

if( millis() > strideTimer ) // time to take a new stride
  {
  static byte stridePhase = 0; // we start from center position, then go through a number of phases
  if(state == STRAIGHT)
    {
    switch(stridePhase)
      {
      case 0: // lift body on one side
        strideTimer += liftInterval;
        moveServo(MIDDLESERVO, middleServoCenter - middleLiftHeight);
        break;
      case 1: // advance to extreme
        strideTimer += strideInterval;   
        moveServo(LEFTSERVO, leftServoCenter + sideStrideLength);
        moveServo(RIGHTSERVO, rightServoCenter + sideStrideLength);
        break;
      case 2: // lift other side of body
        strideTimer += liftInterval;
        moveServo(MIDDLESERVO, middleServoCenter + middleLiftHeight);
        break;
      case 3: // reverse to extreme
        strideTimer += strideInterval;   
        moveServo(LEFTSERVO, leftServoCenter - sideStrideLength);
        moveServo(RIGHTSERVO, rightServoCenter - sideStrideLength);
        break;
      }
    }
  else // turning
    {
    char turnDirection = 1;
    if( state > LEFT ) // if turning or straightening right...
      { turnDirection = -1; } // ...reverse middle legs phase
    switch(stridePhase)
      {
      case 0: // lift body on one side
        strideTimer += liftInterval;
        moveServo(MIDDLESERVO, middleServoCenter - (middleLiftHeight * turnDirection));
        break;
      case 1: // advance to extreme
        strideTimer += strideInterval;   
        moveServo(LEFTSERVO, leftServoCenter + sideStrideLength);
        moveServo(RIGHTSERVO, rightServoCenter - sideStrideLength);
        break;
      case 2: // lift other side of body
        strideTimer += liftInterval;
        moveServo(MIDDLESERVO, middleServoCenter + (middleLiftHeight * turnDirection));
        break;
      case 3: // reverse to extreme
        strideTimer += strideInterval;   
        moveServo(LEFTSERVO, leftServoCenter - sideStrideLength);
        moveServo(RIGHTSERVO, rightServoCenter + sideStrideLength);
        break;
      }
    }

  stridePhase++;
  if( stridePhase > 3 ) // we've completed an entire stride
    {
    stridePhase = 0;
    if( strideCounter > 0 ) // make sure we don't overflow
      { strideCounter--; }
    }
  }
}


[nickgammon]

That's so cute! 

[Peter_I]

That's so cute! 

Yes, cute...... And a little scary in a spidery sort of way.

"Arachnophobia combined with Bambi on the frozen lake"

[indream]

cool,how to turn corner ?

[nickgammon]

Get a cat.

[tylernt]

cool,how to turn corner ?

When walking straight, the middle legs will lift one side the body, and both and the left front/rear legs move in phase with the right front/rear legs. That is, both side servos rotate the same direction (clockwise or counterclockwise) to take a step forward.

Turning is weird. The middle legs do the same thing, but the front/rear legs move in opposite phase -- the front legs get closer together and the rear legs move farther apart, and vice versa. It's hard to describe but the effect is shown in the Pololu video in slow motion.

The fun thing is putting the middle legs in an opposite phase reverses whatever is happening. If you're going straight forward, reversing the middle leg phase makes you walk backwards. If you're turning left, reversing the middle phase makes you turn right.

Actually I think I see a bug, my code has

moveServo(MIDDLESERVO, middleServoCenter - (middleLiftHeight * turnDirection));

but I think it perhaps should be

moveServo(MIDDLESERVO, (middleServoCenter - middleLiftHeight) * turnDirection);

especially since I think I remember it turning faster right than left. I'll have to try changing it. I brought out the reset pin so I can use ICSP, though of course not with a standard header.

"Arachnophobia combined with Bambi on the frozen lake"

Get a cat.

LOL!