From 42c28e0bf77b0bb4c66303be7ebac57e96c2223e Mon Sep 17 00:00:00 2001 From: Marcus Priesch Date: Tue, 9 Apr 2013 13:56:04 +0200 Subject: [PATCH] timer4 servo pwm --- examples/osd/servo-sensor/er-example-server.c | 2 +- platform/osd-merkur/dev/t4-servo-sensor.c | 113 +++++++++++++ platform/osd-merkur/dev/t4-servo-sensor.h | 71 +++++++++ platform/osd-merkur/dev/t4-servo.c | 150 ++++++++++++++++++ platform/osd-merkur/dev/t4-servo.h | 96 +++++++++++ 5 files changed, 431 insertions(+), 1 deletion(-) create mode 100644 platform/osd-merkur/dev/t4-servo-sensor.c create mode 100644 platform/osd-merkur/dev/t4-servo-sensor.h create mode 100644 platform/osd-merkur/dev/t4-servo.c create mode 100644 platform/osd-merkur/dev/t4-servo.h diff --git a/examples/osd/servo-sensor/er-example-server.c b/examples/osd/servo-sensor/er-example-server.c index d8e2b436f..be0a01ef0 100644 --- a/examples/osd/servo-sensor/er-example-server.c +++ b/examples/osd/servo-sensor/er-example-server.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2011-2013, Matthias Kovatsch and other contributors. + * Copyright (C) 2011, Matthias Kovatsch and other contributors. * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/platform/osd-merkur/dev/t4-servo-sensor.c b/platform/osd-merkur/dev/t4-servo-sensor.c new file mode 100644 index 000000000..cbf82306c --- /dev/null +++ b/platform/osd-merkur/dev/t4-servo-sensor.c @@ -0,0 +1,113 @@ +/* +** Copyright (C) 2013 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-sensor +** +** Purpose +** Implements a sensor around t4servo.c +** +** +** Revision Dates +** 31-Mar-2013 (MPR) Creation +** ««revision-date»»··· +**-- +*/ + +#include "contiki.h" +#include "dev/t4-servo.h" +#include "dev/t4-servo-sensor.h" + +const struct sensors_sensor t4_servo_sensor; +static int status(int type); +static int enabled = 0; + +static int value (int channel) + { + if (channel >= SERVO_COUNT) + return -1; + else if (channel < 0) + return -2; + else + return t4_servo_get (channel); + } + +static int configure (int type, int c) // type, c: SENSORS_ACTIVE, 1 -> act. + // type, c: SENSORS_ACTIVE, 0 -> deact. + { + switch (type) + { + case SENSORS_ACTIVE : + if (c == 0) + { + t4_servo_off (); + } + else if (c == 1) + { + t4_servo_init (); + t4_servo_set_io (0, T4_SENSOR_1_PORT, T4_SENSOR_1_DDR, T4_SENSOR_1_PIN); + t4_servo_set_io (1, T4_SENSOR_2_PORT, T4_SENSOR_2_DDR, T4_SENSOR_2_PIN); + t4_servo_set_io (2, T4_SENSOR_3_PORT, T4_SENSOR_3_DDR, T4_SENSOR_3_PIN); + t4_servo_set_io (3, T4_SENSOR_4_PORT, T4_SENSOR_4_DDR, T4_SENSOR_4_PIN); + } + break; + + case 0 : + t4_servo_set (0, c); + break; + + case 1 : + t4_servo_set (1, c); + break; + + case 2 : + t4_servo_set (2, c); + break; + + case 3 : + t4_servo_set (3, c); + + default: + break; + } + return 0; + } + +static int status(int type) + { + switch (type) + { + case SENSORS_ACTIVE: + case SENSORS_READY: + return enabled; + } + return 0; + } + +SENSORS_SENSOR(t4_servo_sensor, T4_SENSOR_NAME, value, configure, status); diff --git a/platform/osd-merkur/dev/t4-servo-sensor.h b/platform/osd-merkur/dev/t4-servo-sensor.h new file mode 100644 index 000000000..bbbf22adf --- /dev/null +++ b/platform/osd-merkur/dev/t4-servo-sensor.h @@ -0,0 +1,71 @@ +/* +** Copyright (C) 2013 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-sensor +** +** Purpose +** Implements a sensor around t4servo.c +** +** +** Revision Dates +** 31-Mar-2013 (MPR) Creation +** ««revision-date»»··· +**-- +*/ + +#ifndef __T4_SERVO_SENSOR_H__ +#define __T4_SERVO_SENSOR_H__ + +#include "lib/sensors.h" + +extern const struct sensors_sensor t4_servo_sensor; + +#define T4_SENSOR_NAME "T4-Sensor" + +#define T4_SENSOR_1_PORT 0x0E +#define T4_SENSOR_1_DDR 0x0D +#define T4_SENSOR_1_PIN PINE4 + +#define T4_SENSOR_2_PORT 0x0E +#define T4_SENSOR_2_DDR 0x0D +#define T4_SENSOR_2_PIN PINE3 + +#define T4_SENSOR_3_PORT 0x0E +#define T4_SENSOR_3_DDR 0x0D +#define T4_SENSOR_3_PIN PINE5 + +#define T4_SENSOR_4_PORT 0x0E +#define T4_SENSOR_4_DDR 0x0D +#define T4_SENSOR_4_PIN PINE6 + +#endif /* __T4_SERVO_SENSOR_H__ */ + + + diff --git a/platform/osd-merkur/dev/t4-servo.c b/platform/osd-merkur/dev/t4-servo.c new file mode 100644 index 000000000..60a6b7027 --- /dev/null +++ b/platform/osd-merkur/dev/t4-servo.c @@ -0,0 +1,150 @@ +/* +** Copyright (C) 2013 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 +** ��revision-date����� +**-- +*/ + +#include "t4-servo.h" +#include +#include +#include "led.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 ++) + { + servo_channels [channel].duty = SERVO_MAX/2; + t4_servo_set_io (channel, DEFAULT_PORT, DEFAULT_DDR, DEFAULT_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; + sei(); +} + +void t4_servo_off (void) + { + TIMSK4_struct.ocie4a = 0; + } + +int t4_servo_set_io + ( unsigned char channel + , unsigned char port + , unsigned char ddr + , unsigned char pin + ) + { + if (channel >= SERVO_COUNT) + return -1; + + servo_channels [channel].port = port; + servo_channels [channel].ddr = ddr; + servo_channels [channel].pin = 1 << pin; + + _SFR_IO8 (ddr) |= pin; + + return 0; + } + +int t4_servo_get (unsigned int channel) + { + if (channel >= SERVO_COUNT) + return -1; + + return servo_channels [channel].duty; + } + +int t4_servo_set (unsigned int channel, unsigned int 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(); + } diff --git a/platform/osd-merkur/dev/t4-servo.h b/platform/osd-merkur/dev/t4-servo.h new file mode 100644 index 000000000..72334a088 --- /dev/null +++ b/platform/osd-merkur/dev/t4-servo.h @@ -0,0 +1,96 @@ +/* +** Copyright (C) 2013 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 +** ��revision-date����� +**-- +*/ + +#ifndef __T4_SERVO_H__ +#define __T4_SERVO_H__ + +// timing: (X:pwm, _:zero) +// +// X_______________XXXXXXXXXXXXX_______________XXXXXXXXXXXXX +// +// |<------------->|<--------->|<------------->|<--------->| ... +// SERVO_OFFSET SERVO_MAX SERVO_OFFSET SERVO_MAX +// +// within SERVO_MAX you get the PWM set with servo_set (id, pwm) +// if you set SERVO_OFFSET to zero, you get a pwm from 0 to 100% duty cycle +// +// the period of the smallest tick is derived from the timer4 prescaler +// plus the ISR overhead. however, the latter one should be constant +// regardless of the pwm signal generated + + +#define SERVO_OFFSET 256 +#define SERVO_MIN 16 +#define SERVO_MAX 32 + +// Counter value for timer 4 without any prescaler for a single tick +#define PWMFREQ 500 +#define T4_VALUE F_CPU/SERVO_MAX/PWMFREQ + +#define SERVO_COUNT 4 + +#define DEFAULT_PORT 0x0E +#define DEFAULT_PIN 1 +#define DEFAULT_DDR 0x0D + +typedef struct struct_servo_channel +{ + unsigned char port; + unsigned char pin; + unsigned char ddr; + unsigned char duty; +} servo_channel_type; + +void t4_servo_init(void); +void t4_servo_off(void); + +int t4_servo_get(unsigned int channel); +int t4_servo_set(unsigned int channel, unsigned int duty); + +int t4_servo_set_io + ( unsigned char channel + , unsigned char port + , unsigned char ddr + , unsigned char pin + ); + +#endif /* __T4_SERVO_H__ */