Subversion Repositories Projects

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1198 - 1
 
2
/********************************************************************/
3
/*                                                                                                                                      */
4
/*                               NG-Video 5,8GHz                                                                */
5
/*                                                                                                                                      */
6
/*                              Copyright (C) 2011 - gebad                                                      */
7
/*                                                                                                                                      */
8
/*  This code is distributed under the GNU Public License                       */
9
/*      which can be found at http://www.gnu.org/licenses/gpl.txt               */
10
/*                                                                                                                                      */
11
/*      using                                                                                                                   */
12
/*! \file servo.c \brief Interrupt-driven RC Servo function library.*/
13
/*                                                                                                                                      */
14
/*File Name     : 'servo.c'                                                                                             */
15
/*Title         : Interrupt-driven RC Servo function library                    */
16
/*Author                : Pascal Stang - Copyright (C) 2002                                     */
17
/*Created               : 7/31/2002                                                                                     */
18
/*Revised               : 8/02/2002                                                                                     */
19
/*Version               : 1.0                                                                                           */
20
/*Target MCU    : Atmel AVR Series                                                                      */
21
/*Editor Tabs   : 4                                                                                                     */
22
/*                                                                                                                                      */
23
/*This code is distributed under the GNU Public License                         */
24
/*              which can be found at http://www.gnu.org/licenses/gpl.txt       */
25
/*                                                                                                                                      */
26
/********************************************************************/
27
 
28
#include <avr/interrupt.h>
29
#include <avr/io.h>
30
 
31
#include "servo.h"
32
#include "config.h"
33
 
34
// Global variables
35
uint16_t ServoPosTics;
36
uint16_t ServoPeriodTics;
37
 
38
// servo channel registers
39
ServoChannelType ServoChannels[SERVO_NUM_CHANNELS];
40
ServoConst_t ServoConst[2] = {{SERVO_MAX, SERVO_MIN, SERVO_STEPS, SERVO_PRESCALER},\
41
                                                          {SERVO_MAX * 4, SERVO_MIN * 4, (SERVO_STEPS + 1) * 4 - 1, SERVO_PRESCALER / 4}};
42
 
43
 
44
// functions
45
 
46
//! initializes software PWM system 16-bit Timer
47
// normaler Weise wird ein Serv-PWM Signal aller 20ms wiederholt
48
// Werte: rev, min, max, mid vorher über servoSet...() initialisieren und einmal servoSetPosition(...) ausführen!!!
49
void servoInit(uint8_t servo_period)
50
{ uint16_t OCValue; // set initial interrupt time
51
 
52
  // disble Timer/Counter1, Output Compare A Match Interrupt
53
  TIMSK1 &= ~(1<<OCIE1A);
54
  // set the prescaler for timer1
55
  if (sIdxSteps == STEPS_255) {
56
        TCCR1B &= ~((1<<CS11) | (1<<CS10));
57
    TCCR1B |= (1<<CS12);                                // prescaler 256, Servo-Schritte 185 bei 180 grd Winkel
58
  }
59
  else {
60
    TCCR1B &= ~(1<<CS12);
61
    TCCR1B |= (1<<CS11) | (1<<CS10);    // prescaler 64,  Servo-Schritte 740 bei 180 grd Winkel
62
  }
63
  // attach the software PWM service routine to timer1 output compare A
64
  // timerAttach(TIMER1OUTCOMPAREA_INT, servoService);
65
  // enable channels
66
  for(uint8_t channel=0; channel < SERVO_NUM_CHANNELS; channel++) {
67
        // set default pins assignments SERVO2 Pin 4; SERVO1 Pin 5
68
        ServoChannels[channel].pin = (1 << (SERVO2 + channel));
69
  }
70
  ServoPosTics = 0; // set PosTics
71
  // set PeriodTics
72
  ServoPeriodTics = F_CPU / ServoConst[sIdxSteps].prescaler * servo_period * 1e-3;
73
  // read in current value of output compare register OCR1A
74
  OCValue =  OCR1AL;                    // read low byte of OCR1A
75
  OCValue += (OCR1AH << 8);     // read high byte of OCR1A
76
  OCValue += ServoPeriodTics;   // increment OCR1A value by nextTics
77
  // set future output compare time to this new value
78
  OCR1AH = OCValue >> 8;                // write high byte
79
  OCR1AL = OCValue & 0x00FF;    // write low byte
80
  TIMSK1 |= (1<<OCIE1A);                // enable the timer1 output compare A interrupt
81
}
82
 
83
uint16_t pw_us(uint16_t Steps)
84
{
85
  return(Steps * ServoConst[sIdxSteps].prescaler/(F_CPU * 1e-6)  + 0.5);  // Zeit pro Schritt (Wert * e-1) in µs 
86
}
87
 
88
uint16_t ServoSteps(void)
89
{
90
  return(ServoConst[sIdxSteps].steps);
91
}
92
 
93
void servoSet_rev(uint8_t channel, uint8_t val)
94
{
95
  ServoChannels[channel].rev = val & 0x01;
96
}
97
 
98
void servoSet_min(uint8_t channel, uint16_t min)
99
{
100
  ServoChannels[channel].min = ServoConst[sIdxSteps].min + min;
101
}
102
 
103
void servoSet_max(uint8_t channel, uint16_t max)
104
{
105
  ServoChannels[channel].max = ServoConst[sIdxSteps].max - max;
106
}
107
 
108
void servoSet_mid(uint8_t channel, uint16_t mid)
109
{
110
  ServoChannels[channel].mid = mid;
111
  // Faktor 16, bzw. 16/2 um mit einer Nachkommastelle zu Rechnen 
112
  ServoChannels[channel].mid_scaled = (8 * (ServoChannels[channel].max - ServoChannels[channel].min) + \
113
                                                                          (16 * mid - 8 * ServoConst[sIdxSteps].steps))/16 + ServoChannels[channel].min;
114
}
115
 
116
//! turns off software PWM system
117
void servoOff(void)
118
{
119
  // disable the timer1 output compare A interrupt
120
  TIMSK1 &= ~(1<<OCIE1A);
121
}
122
 
123
//! set servo position on channel
124
// input should be between 0 and ServoSteps (entspricht Maximalausschlag)
125
void servoSetPosition(uint8_t channel, uint16_t position)
126
{ uint16_t pos_scaled;
127
 
128
  // calculate scaled position
129
  if (ServoChannels[channel].rev != 0) position = ServoConst[sIdxSteps].steps - position;
130
  if (position < ServoConst[sIdxSteps].steps/2)
131
    //bei Position < Servomittelstellung  Position*(Mitte - Min)/(Servoschritte/2)
132
    pos_scaled = ServoChannels[channel].min + ((int32_t)position*2*(ServoChannels[channel].mid_scaled-ServoChannels[channel].min))/\
133
                                 ServoConst[sIdxSteps].steps;
134
  else
135
        //bei Position >= Servomittelstellung
136
        pos_scaled = ServoChannels[channel].mid_scaled + (uint32_t)(position - ServoConst[sIdxSteps].steps / 2) \
137
                     * 2 * (ServoChannels[channel].max-ServoChannels[channel].mid_scaled)/ServoConst[sIdxSteps].steps;
138
  // set position
139
  servoSetPositionRaw(channel, pos_scaled);
140
}
141
 
142
//! set servo position on channel (raw unscaled format)
143
void servoSetPositionRaw(uint8_t channel, uint16_t position)
144
{
145
  // bind to limits
146
  if (position < ServoChannels[channel].min) position = ServoChannels[channel].min;
147
  if (position > ServoChannels[channel].max) position = ServoChannels[channel].max;
148
  // set position
149
  ServoChannels[channel].duty = position;
150
}
151
 
152
//Interruptroutine
153
ISR(TIMER1_COMPA_vect)
154
{ static uint8_t ServoChannel;
155
  uint16_t nextTics;
156
  uint16_t OCValue; // schedule next interrupt
157
 
158
  if(ServoChannel < SERVO_NUM_CHANNELS) {
159
        PORTD &= ~ServoChannels[ServoChannel].pin;      // turn off current channel
160
  }
161
  ServoChannel++;  // next channel
162
  if(ServoChannel != SERVO_NUM_CHANNELS) {
163
        // loop to channel 0 if needed
164
        if(ServoChannel > SERVO_NUM_CHANNELS)   ServoChannel = 0;
165
        // turn on new channel
166
        PORTD |= ServoChannels[ServoChannel].pin;
167
        // schedule turn off time
168
        nextTics = ServoChannels[ServoChannel].duty;
169
  }
170
  else {
171
        // ***we could save time by precalculating this
172
        // schedule end-of-period
173
        nextTics = ServoPeriodTics-ServoPosTics;
174
  }
175
  // read in current value of output compare register OCR1A
176
  OCValue =  OCR1AL;                    // read low  byte of OCR1A
177
  OCValue += (OCR1AH <<8);              // read high byte of OCR1A
178
  OCValue += nextTics;                  // increment OCR1A value by nextTics
179
  // set future output compare time to this new value
180
  OCR1AH = OCValue >> 8;                // write high byte
181
  OCR1AL = OCValue & 0x00FF;    // write low  byte
182
 
183
  ServoPosTics += nextTics;             // set our new tic position
184
  if(ServoPosTics >= ServoPeriodTics) ServoPosTics = 0;
185
}
186