140 lines
4.1 KiB
C
140 lines
4.1 KiB
C
|
/*
|
||
|
|
** Copyright (C) 2013-2014 Marcus Priesch, All rights reserved
|
||
|
|
** In Prandnern 31, A--2122 Riedenthal, Austria. office@priesch.co.at
|
||
|
|
**
|
||
|
|
** Redistribution and use in source and binary forms, with or without
|
||
|
|
** modification, are permitted provided that the following conditions
|
||
|
|
** are met:
|
||
|
|
** 1. Redistributions of source code must retain the above copyright
|
||
|
|
** notice, this list of conditions and the following disclaimer.
|
||
|
|
** 2. Redistributions in binary form must reproduce the above copyright
|
||
|
|
** notice, this list of conditions and the following disclaimer in the
|
||
|
|
** documentation and/or other materials provided with the distribution.
|
||
|
|
** 3. Neither the name of the Institute nor the names of its contributors
|
||
|
|
** may be used to endorse or promote products derived from this software
|
||
|
|
** without specific prior written permission.
|
||
|
|
**
|
||
|
|
** THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND
|
||
|
|
** ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||
|
|
** IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||
|
|
** ARE DISCLAIMED. IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
|
||
|
|
** FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||
|
|
** DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
|
||
|
|
** OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||
|
|
** HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||
|
|
** LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
|
||
|
|
** OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
||
|
|
** SUCH DAMAGE.
|
||
|
|
**
|
||
|
|
**++
|
||
|
|
** Name
|
||
|
|
** t4-servo
|
||
|
|
**
|
||
|
|
** Purpose
|
||
|
|
** Implements software pwm on any portpins via timer 4
|
||
|
|
**
|
||
|
|
**
|
||
|
|
** Revision Dates
|
||
|
|
** 31-Mar-2013 (MPR) Creation
|
||
|
|
** 12-Mar-2014 (MPR) Factored to support configurable amount of pwm's
|
||
|
|
** ««revision-date»»···
|
||
|
|
**--
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "t4-servo.h"
|
||
|
|
#include "t4-servo-config.h"
|
||
|
|
#include "t4-servo-hardware.h"
|
||
|
|
#include <avr/io.h>
|
||
|
|
#include <avr/interrupt.h>
|
||
|
|
|
||
|
|
// static servo_channel_type servo_channels [SERVO_COUNT];
|
||
|
|
|
||
|
|
// timer 4: CTC OCR4A
|
||
|
|
#define WGM4 0x4
|
||
|
|
|
||
|
|
void t4_servo_init (void)
|
||
|
|
{
|
||
|
|
unsigned char channel;
|
||
|
|
|
||
|
|
for (channel = 0; channel < SERVO_COUNT; channel ++)
|
||
|
|
{
|
||
|
|
_SFR_IO8 (servo_channels [channel].ddr ) |= servo_channels [channel].pin;
|
||
|
|
_SFR_IO8 (servo_channels [channel].port) &= ~(servo_channels [channel].pin);
|
||
|
|
}
|
||
|
|
|
||
|
|
cli ();
|
||
|
|
TCCR4A = 0x00;
|
||
|
|
TCCR4A_struct.wgm4 = WGM4 & 0x3;
|
||
|
|
TCCR4B_struct.wgm4 = (WGM4 & 0xc) >> 2;
|
||
|
|
TCCR4B_struct.cs4 = 0x1; // No prescaler
|
||
|
|
TCCR4C = 0x00;
|
||
|
|
OCR4A = (T4_VALUE);
|
||
|
|
TIMSK4_struct.ocie4a = 1;
|
||
|
|
TIMSK4_struct.toie4 = 1;
|
||
|
|
sei();
|
||
|
|
}
|
||
|
|
|
||
|
|
void t4_servo_off (void)
|
||
|
|
{
|
||
|
|
TIMSK4_struct.toie4 = 0;
|
||
|
|
TIMSK4_struct.ocie4a = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
int t4_servo_get (unsigned int channel)
|
||
|
|
{
|
||
|
|
if (channel >= SERVO_COUNT)
|
||
|
|
return -1;
|
||
|
|
//printf ("t4_servo_get: %d, %d\n", channel, servo_channels [channel].duty);
|
||
|
|
return servo_channels [channel].duty;
|
||
|
|
}
|
||
|
|
|
||
|
|
int t4_servo_set (unsigned int channel, unsigned char duty)
|
||
|
|
{
|
||
|
|
//printf ("t4_servo_set: %d, %d\n", channel, duty);
|
||
|
|
|
||
|
|
if (channel >= SERVO_COUNT)
|
||
|
|
return -1;
|
||
|
|
|
||
|
|
if (duty > SERVO_MAX)
|
||
|
|
return -2;
|
||
|
|
|
||
|
|
if (duty < SERVO_MIN)
|
||
|
|
return -3;
|
||
|
|
|
||
|
|
servo_channels [channel].duty = duty;
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
ISR (TIMER4_COMPA_vect, ISR_NOBLOCK)
|
||
|
|
{
|
||
|
|
unsigned char channel;
|
||
|
|
static unsigned int tick_count = 0;
|
||
|
|
|
||
|
|
cli ();
|
||
|
|
for (channel = 0; channel < SERVO_COUNT; channel ++)
|
||
|
|
{
|
||
|
|
if (tick_count < servo_channels [channel].duty)
|
||
|
|
{
|
||
|
|
// turn on
|
||
|
|
// _SFR_IO8 (servo_channels [channel].ddr ) |= servo_channels [channel].pin;
|
||
|
|
_SFR_IO8 (servo_channels [channel].port) |= servo_channels [channel].pin;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
// turn off
|
||
|
|
_SFR_IO8 (servo_channels [channel].port) &= ~(servo_channels [channel].pin);
|
||
|
|
// _SFR_IO8 (servo_channels [channel].ddr ) |= servo_channels [channel].pin;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
tick_count ++;
|
||
|
|
|
||
|
|
if (tick_count >= (SERVO_MAX + SERVO_OFFSET))
|
||
|
|
{
|
||
|
|
tick_count = 0;
|
||
|
|
}
|
||
|
|
sei();
|
||
|
|
|
||
|
|
}
|
||
|
|
|