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.
// 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.
}