Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
962 | - | 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 |
||
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(void) |
||
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_PERIODE * 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 ServoSteps(void) |
||
84 | { |
||
85 | return(ServoConst[sIdxSteps].steps); |
||
86 | } |
||
87 | |||
88 | void servoSet_rev(uint8_t channel, uint8_t val) |
||
89 | { |
||
90 | ServoChannels[channel].rev = val & 0x01; |
||
91 | } |
||
92 | |||
93 | void servoSet_min(uint8_t channel, uint16_t min) |
||
94 | { |
||
95 | ServoChannels[channel].min = ServoConst[sIdxSteps].min + min; |
||
96 | } |
||
97 | |||
98 | void servoSet_max(uint8_t channel, uint16_t max) |
||
99 | { |
||
100 | ServoChannels[channel].max = ServoConst[sIdxSteps].max - max; |
||
101 | } |
||
102 | |||
103 | void servoSet_mid(uint8_t channel, uint16_t mid) |
||
104 | { |
||
105 | ServoChannels[channel].mid = mid; |
||
106 | ServoChannels[channel].mid_scaled = (float)(ServoChannels[channel].max - ServoChannels[channel].min)/2 + \ |
||
107 | (mid - (float)ServoConst[sIdxSteps].steps/2) + ServoChannels[channel].min; |
||
108 | } |
||
109 | |||
110 | //! turns off software PWM system |
||
111 | void servoOff(void) |
||
112 | { |
||
113 | // disable the timer1 output compare A interrupt |
||
114 | TIMSK1 &= ~(1<<OCIE1A); |
||
115 | } |
||
116 | |||
117 | //! set servo position on channel |
||
118 | // input should be between 0 and ServoSteps (entspricht Maximalausschlag) |
||
119 | void servoSetPosition(uint8_t channel, uint16_t position) |
||
120 | { uint16_t pos_scaled; |
||
121 | |||
122 | // calculate scaled position |
||
123 | if (ServoChannels[channel].rev != 0) position = ServoConst[sIdxSteps].steps - position; |
||
124 | if (position < ServoConst[sIdxSteps].steps/2) |
||
125 | //bei Position < Servomittelstellung Position*(Mitte - Min)/(Servoschritte/2) |
||
126 | pos_scaled = ((uint32_t)position*2*(float)(ServoChannels[channel].mid_scaled-ServoChannels[channel].min)/\ |
||
127 | ServoConst[sIdxSteps].steps)+ServoChannels[channel].min; |
||
128 | else |
||
129 | //bei Position >= Servomittelstellung |
||
130 | pos_scaled = ServoChannels[channel].mid_scaled + (uint32_t)((position - ((float)ServoConst[sIdxSteps].steps/2)) \ |
||
131 | * 2 * (float)(ServoChannels[channel].max-ServoChannels[channel].mid_scaled)/ServoConst[sIdxSteps].steps); |
||
132 | // set position |
||
133 | servoSetPositionRaw(channel, pos_scaled); |
||
134 | } |
||
135 | |||
136 | //! set servo position on channel (raw unscaled format) |
||
137 | void servoSetPositionRaw(uint8_t channel, uint16_t position) |
||
138 | { |
||
139 | // bind to limits |
||
140 | if (position < ServoChannels[channel].min) position = ServoChannels[channel].min; |
||
141 | if (position > ServoChannels[channel].max) position = ServoChannels[channel].max; |
||
142 | // set position |
||
143 | ServoChannels[channel].duty = position; |
||
144 | } |
||
145 | |||
146 | //Interruptroutine |
||
147 | ISR(TIMER1_COMPA_vect) |
||
148 | { static uint8_t ServoChannel; |
||
149 | uint16_t nextTics; |
||
150 | uint16_t OCValue; // schedule next interrupt |
||
151 | |||
152 | if(ServoChannel < SERVO_NUM_CHANNELS) { |
||
153 | PORTD &= ~ServoChannels[ServoChannel].pin; // turn off current channel |
||
154 | } |
||
155 | ServoChannel++; // next channel |
||
156 | if(ServoChannel != SERVO_NUM_CHANNELS) { |
||
157 | // loop to channel 0 if needed |
||
158 | if(ServoChannel > SERVO_NUM_CHANNELS) ServoChannel = 0; |
||
159 | // turn on new channel |
||
160 | PORTD |= ServoChannels[ServoChannel].pin; |
||
161 | // schedule turn off time |
||
162 | nextTics = ServoChannels[ServoChannel].duty; |
||
163 | } |
||
164 | else { |
||
165 | // ***we could save time by precalculating this |
||
166 | // schedule end-of-period |
||
167 | nextTics = ServoPeriodTics-ServoPosTics; |
||
168 | } |
||
169 | // read in current value of output compare register OCR1A |
||
170 | OCValue = OCR1AL; // read low byte of OCR1A |
||
171 | OCValue += (OCR1AH <<8); // read high byte of OCR1A |
||
172 | OCValue += nextTics; // increment OCR1A value by nextTics |
||
173 | // set future output compare time to this new value |
||
174 | OCR1AH = OCValue >> 8; // write high byte |
||
175 | OCR1AL = OCValue & 0x00FF; // write low byte |
||
176 | |||
177 | ServoPosTics += nextTics; // set our new tic position |
||
178 | if(ServoPosTics >= ServoPeriodTics) ServoPosTics = 0; |
||
179 | } |
||
180 |