mirror of
https://github.com/arduino/Arduino.git
synced 2024-12-10 21:24:12 +01:00
170 lines
5.1 KiB
C
Executable File
170 lines
5.1 KiB
C
Executable File
/*! \file servo.c \brief Interrupt-driven RC Servo function library. */
|
|
//*****************************************************************************
|
|
//
|
|
// File Name : 'servo.c'
|
|
// Title : Interrupt-driven RC Servo function library
|
|
// Author : Pascal Stang - Copyright (C) 2002
|
|
// Created : 7/31/2002
|
|
// Revised : 8/02/2002
|
|
// Version : 1.0
|
|
// Target MCU : Atmel AVR Series
|
|
// Editor Tabs : 4
|
|
//
|
|
// This code is distributed under the GNU Public License
|
|
// which can be found at http://www.gnu.org/licenses/gpl.txt
|
|
//
|
|
//*****************************************************************************
|
|
|
|
#ifndef WIN32
|
|
#include <avr/io.h>
|
|
#endif
|
|
|
|
#include "global.h"
|
|
#include "servo.h"
|
|
|
|
// Program ROM constants
|
|
|
|
// Global variables
|
|
// servo channel registers
|
|
u16 ServoPosTics;
|
|
u16 ServoPeriodTics;
|
|
u08 ServoChannel;
|
|
ServoChannelType ServoChannels[SERVO_NUM_CHANNELS];
|
|
|
|
// functions
|
|
|
|
//! initializes software PWM system
|
|
void servoInit(void)
|
|
{
|
|
u08 channel;
|
|
// disble the timer1 output compare A interrupt
|
|
cbi(TIMSK, OCIE1A);
|
|
// set the prescaler for timer1
|
|
timer1SetPrescaler(TIMER_CLK_DIV256);
|
|
// attach the software PWM service routine to timer1 output compare A
|
|
timerAttach(TIMER1OUTCOMPAREA_INT, servoService);
|
|
// enable and clear channels
|
|
for(channel=0; channel<SERVO_NUM_CHANNELS; channel++)
|
|
{
|
|
// set minimum position as default
|
|
ServoChannels[channel].duty = SERVO_MIN;
|
|
// set default port and pins assignments
|
|
ServoChannels[channel].port = _SFR_IO_ADDR(SERVO_DEFAULT_PORT);
|
|
//ServoChannels[channel].port = (unsigned char)&SERVO_DEFAULT_PORT;
|
|
ServoChannels[channel].pin = (1<<channel);
|
|
// set channel pin to output
|
|
// THIS IS OBSOLETED BY THE DYNAMIC CHANNEL TO PORT,PIN ASSIGNMENTS
|
|
//outb(SERVODDR, inb(SERVODDR) | (1<<channel));
|
|
}
|
|
// set PosTics
|
|
ServoPosTics = 0;
|
|
// set PeriodTics
|
|
ServoPeriodTics = SERVO_MAX*9;
|
|
// set initial interrupt time
|
|
u16 OCValue;
|
|
// read in current value of output compare register OCR1A
|
|
OCValue = inb(OCR1AL); // read low byte of OCR1A
|
|
OCValue += inb(OCR1AH)<<8; // read high byte of OCR1A
|
|
// increment OCR1A value by nextTics
|
|
OCValue += ServoPeriodTics;
|
|
// set future output compare time to this new value
|
|
outb(OCR1AH, (OCValue>>8)); // write high byte
|
|
outb(OCR1AL, (OCValue & 0x00FF)); // write low byte
|
|
// enable the timer1 output compare A interrupt
|
|
sbi(TIMSK, OCIE1A);
|
|
}
|
|
|
|
//! turns off software PWM system
|
|
void servoOff(void)
|
|
{
|
|
// disable the timer1 output compare A interrupt
|
|
cbi(TIMSK, OCIE1A);
|
|
// detach the service routine
|
|
timerDetach(TIMER1OUTCOMPAREA_INT);
|
|
}
|
|
|
|
//! set port and I/O pin for channel
|
|
void servoSetChannelIO(u08 channel, u08 port, u08 pin)
|
|
{
|
|
ServoChannels[channel].port = port;
|
|
ServoChannels[channel].pin = (1<<(pin&0x07));
|
|
}
|
|
|
|
//! set servo position on channel
|
|
void servoSetPosition(u08 channel, u08 position)
|
|
{
|
|
// input should be between 0 and SERVO_POSITION_MAX
|
|
u16 pos_scaled;
|
|
// calculate scaled position
|
|
pos_scaled = ((u16)position*(SERVO_MAX-SERVO_MIN)/SERVO_POSITION_MAX)+SERVO_MIN;
|
|
// set position
|
|
servoSetPositionRaw(channel, pos_scaled);
|
|
}
|
|
|
|
//! get servo position on channel
|
|
u08 servoGetPosition(u08 channel)
|
|
{
|
|
return (u08)( ((servoGetPositionRaw(channel)-SERVO_MIN)*SERVO_POSITION_MAX)/(SERVO_MAX-SERVO_MIN) );
|
|
}
|
|
|
|
//! set servo position on channel (raw unscaled format)
|
|
void servoSetPositionRaw(u08 channel, u16 position)
|
|
{
|
|
// bind to limits
|
|
position = MAX(position, SERVO_MIN);
|
|
position = MIN(position, SERVO_MAX);
|
|
// set position
|
|
ServoChannels[channel].duty = position;
|
|
}
|
|
|
|
//! get servo position on channel (raw unscaled format)
|
|
u16 servoGetPositionRaw(u08 channel)
|
|
{
|
|
return ServoChannels[channel].duty;
|
|
}
|
|
|
|
void servoService(void)
|
|
{
|
|
u16 nextTics;
|
|
|
|
if(ServoChannel < SERVO_NUM_CHANNELS)
|
|
{
|
|
// turn off current channel
|
|
outb(_SFR_IO8(ServoChannels[ServoChannel].port), inb(_SFR_IO8(ServoChannels[ServoChannel].port)) & ~(ServoChannels[ServoChannel].pin));
|
|
}
|
|
|
|
// next channel
|
|
ServoChannel++;
|
|
|
|
if(ServoChannel != SERVO_NUM_CHANNELS)
|
|
{
|
|
// loop to channel 0 if needed
|
|
if(ServoChannel > SERVO_NUM_CHANNELS) ServoChannel = 0;
|
|
// turn on new channel
|
|
outb(_SFR_IO8(ServoChannels[ServoChannel].port), inb(_SFR_IO8(ServoChannels[ServoChannel].port)) | (ServoChannels[ServoChannel].pin));
|
|
// schedule turn off time
|
|
nextTics = ServoChannels[ServoChannel].duty;
|
|
}
|
|
else //(Channel == SERVO_NUM_CHANNELS)
|
|
{
|
|
// ***we could save time by precalculating this
|
|
// schedule end-of-period
|
|
nextTics = ServoPeriodTics-ServoPosTics;
|
|
}
|
|
|
|
// schedule next interrupt
|
|
u16 OCValue;
|
|
// read in current value of output compare register OCR1A
|
|
OCValue = inb(OCR1AL); // read low byte of OCR1A
|
|
OCValue += inb(OCR1AH)<<8; // read high byte of OCR1A
|
|
// increment OCR1A value by nextTics
|
|
OCValue += nextTics;
|
|
// OCR1A+=nextTics;
|
|
// set future output compare time to this new value
|
|
outb(OCR1AH, (OCValue>>8)); // write high byte
|
|
outb(OCR1AL, (OCValue & 0x00FF)); // write low byte
|
|
// set our new tic position
|
|
ServoPosTics += nextTics;
|
|
if(ServoPosTics >= ServoPeriodTics) ServoPosTics = 0;
|
|
}
|