4 Servos (on) 1 Timer (hehehe), Efficient and Reasonable Resolution

[frank26080115]

I can now control 4 servos using just one hardware timer, timers run in the background so your main program code and execute at top speed.
Your MCU must have at least one 8-bit timer for this to work. Any pin can be used. Current configuration is for ATMEGA168 at 16MHz, my servos are running on 7.2V so your timing may need tweaking (see tweak section in code).

Code: [Select]

// Standard AVR Headers
#include <avr/io.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <inttypes.h>
#include <avr/interrupt.h>

// Macros
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) // clear bit
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) // set bit

// General Compiler Instruction
#define F_CPU 16000000L
#define __AVR_ATmega168__

// end MCU and compiler configurations

// begin servo settings and functions

// pin definitions
// drive on timer 0
#define drivePort PORTD
#define driveDDR DDRD
#define leftDrivePin 4 // compare match A
#define rightDrivePin 3 // compare match B

// head on timer 0
#define headPort PORTD
#define headDDR DDRD
#define xHeadPin 5 // compare match B
#define yHeadPin 6 // compare match A

// tweaks for 16MHz
// trial and error needed here
#define driveCenter 100
#define driveLeftRange 30
#define driveRightRange 30
#define leftOffset -1
#define rightOffset -1

#define headCenter 100
#define headXRange 30
#define headYRange 30
#define xOffset -1
#define yOffset -1
#define xTimerUpperLimit 130
#define yTimerUpperLimit 130
#define xTimerLowerLimit 60
#define yTimerLowerLimit 60

// variables
volatile uint8_t servoPulseFlag0 = 0;
volatile uint8_t leftDriveServoTime;
volatile uint8_t rightDriveServoTime;
volatile uint8_t xHeadServoTime;
volatile uint8_t yHeadServoTime;

ISR(SIG_OVERFLOW0)
{
// alternate between drive and head servos
servoPulseFlag0++;
if(bit_is_clear(servoPulseFlag0, 0))
{
// drive servo
OCR0A = leftDriveServoTime;
OCR0B = rightDriveServoTime;
// it's better to leave pin off if servo needs to be stopped
if(OCR0A != (driveCenter + leftOffset))
{
sbi(drivePort, leftDrivePin);
}
if(OCR0B != (driveCenter + rightOffset))
{
sbi(drivePort, rightDrivePin);
}
}
else
{
// head servo
sbi(headPort, xHeadPin);
sbi(headPort, yHeadPin);
OCR0B = xHeadServoTime;
OCR0A = yHeadServoTime;
}
}

// after the right time has passed, end pulse
ISR(SIG_OUTPUT_COMPARE0A)
{
cbi(drivePort, leftDrivePin);
cbi(headPort, yHeadPin);
}

ISR(SIG_OUTPUT_COMPARE0B)
{
cbi(drivePort, rightDrivePin);
cbi(headPort, xHeadPin);
}

void servoInit()
{
// clear ports
cbi(drivePort, leftDrivePin);
cbi(drivePort, rightDrivePin);
cbi(headPort, xHeadPin);
cbi(headPort, yHeadPin);
// set as outputs
sbi(driveDDR, leftDrivePin);
sbi(driveDDR, rightDrivePin);
sbi(headDDR, xHeadPin);
sbi(headDDR, yHeadPin);

// start the timer
TCCR0B = 0b00000100; // timer start, divide by 256
TIMSK0 = 0b00000111; // interrupts on
}

void servoDrive(signed int leftSpeed, signed int rightSpeed)
{
// limit, or suffer errors
if(leftSpeed < -127)
{
leftSpeed = 127;
}
else if(leftSpeed > 127)
{
leftSpeed = 127;
}
if(rightSpeed < -127)
{
rightSpeed = 127;
}
else if(rightSpeed > 127)
{
rightSpeed = 127;
}

signed int leftTimer = leftOffset + driveCenter + ((driveRightRange * leftSpeed) / 128);
signed int rightTimer = rightOffset + driveCenter - ((driveRightRange * rightSpeed) / 128);

leftDriveServoTime = leftTimer;
rightDriveServoTime = rightTimer;
}

void servoDriveMixed(signed int xAxis, signed int yAxis)
{
signed int leftSpeed = 2 * ((yAxis / 2) + (xAxis / 2));
    signed int rightSpeed = 2 * ((yAxis / 2) - (xAxis / 2));

servoDrive(leftSpeed, rightSpeed);
}

void servoHead(int xAxis, int yAxis)
{
signed int xTimer = xOffset + headCenter + ((headXRange * xAxis) / 127);
signed int yTimer = yOffset + headCenter + ((headYRange * yAxis) / 127);

// limit, or suffer errors
if(xTimer > xTimerUpperLimit)
{
xTimer = xTimerUpperLimit;
}
else if(xTimer < xTimerLowerLimit)
{
xTimer = xTimerLowerLimit;
}

if(yTimer > yTimerUpperLimit)
{
yTimer = yTimerUpperLimit;
}
else if(yTimer < yTimerLowerLimit)
{
yTimer = yTimerLowerLimit;
}

xHeadServoTime = xTimer;
yHeadServoTime = yTimer;
}

void servoDisable()
{
TCCR0B = 0; // stop timer
// turn off the pins
cbi(drivePort, leftDrivePin);
cbi(drivePort, rightDrivePin);
cbi(headPort, xHeadPin);
cbi(headPort, yHeadPin);
}

// end servo functions

void mainInit()
{
sei();
servoInit();
servoDrive(0, 0);
servoHead(0, 0);
}

void mainLoop()
{
int i;
int j;

for(j = -127; j <= 127; j++)
{
for(i = 0; i < 25; i++) // change 25 to change delay between interval
{
servoDrive(j, j);
servoHead(j, j);
}
}
for(j = 127; j >= -127; j--)
{
for(i = 0; i < 25; i++)
{
servoDrive(j, j);
servoHead(j, j);
}
}
}

int main()
{
mainInit();
while(1)
{
mainLoop();
}
return 0;
}
servoInit is the function that starts the timers
servoDrive takes two signed integers between -127 and 127, 0 being stop, 127 being forward. It's made for modified servos only.
servoHead is the same, with 0 being 90 center

You only need to call these functions if you need to change the angle or speed, it will automatically send the pulse repeatedly if you don't do anything.

Please give me feedback!! Also leave feedback for http://www.societyofrobots.com/robotforum/index.php?topic=3380.0