root / branches / library_refactor / projects / colonet / robot / dragonfly_wireless_relay / lights.c @ 1390
History | View | Annotate | Download (8.05 KB)
1 |
/*
|
---|---|
2 |
lights.c
|
3 |
Controls orb1 and orb2. Also contains the framework for a software PWM that may be used for servos in the future.
|
4 |
|
5 |
author: CMU Robotics Club, Colony Project
|
6 |
|
7 |
Change Log:
|
8 |
2.4.07 - Aaron
|
9 |
Revamped orb code so it works. Need to check interaction with rtc, and tweak some colors.
|
10 |
|
11 |
2.1.07 - James
|
12 |
Modified sort_buffer() to prune for repeats. PWM now uses orb_buf_size for the number of orb values in orb_time_arr[].
|
13 |
Changed sorting algorithm used in sort_buffer() to selection sort (faster). And it works now.
|
14 |
|
15 |
1.25.07 - KWoo
|
16 |
Deleted old FF+ code to make it cleaner. Commented code. This all works. Note however that if you ever plan to use the
|
17 |
software PWM (which is this) you will need to change the implementation of orb_enable() and orb_disable() to not
|
18 |
shutdown the PWM.
|
19 |
|
20 |
*/
|
21 |
|
22 |
#include "lights.h" |
23 |
#include "dragonfly_lib.h" |
24 |
|
25 |
#define ORB_COUNT 8 //please dont change this, or bad things might happen |
26 |
|
27 |
struct ORB_NODE {
|
28 |
uint8_t num; |
29 |
uint16_t angle; |
30 |
}; |
31 |
|
32 |
struct ORB_CHANGE {
|
33 |
uint16_t port_val; |
34 |
uint16_t split_time_period; |
35 |
}; |
36 |
|
37 |
|
38 |
struct ORB_STATUS_STRUCT {
|
39 |
struct ORB_NODE orbs[ORB_COUNT];
|
40 |
uint16_t orb_angles[ORB_COUNT]; |
41 |
struct ORB_CHANGE changes[ORB_COUNT+1]; |
42 |
uint8_t change_count; |
43 |
uint8_t new_angles; |
44 |
uint8_t current_orb; |
45 |
|
46 |
} ORB_STATUS; |
47 |
|
48 |
void orb_sort(void); |
49 |
void orb_setup_pulse(void); |
50 |
|
51 |
SIGNAL (SIG_OUTPUT_COMPARE3C){ |
52 |
|
53 |
//pull the correct ones down
|
54 |
ORBPORT &= (~ORBMASK)|(ORB_STATUS.changes[ORB_STATUS.current_orb].port_val); |
55 |
|
56 |
++ORB_STATUS.current_orb; //now look at next orb transition
|
57 |
|
58 |
if (ORB_STATUS.current_orb < ORB_STATUS.change_count) { //if it isnt the end... |
59 |
|
60 |
//setup timer for next pull down
|
61 |
OCR3C = TCNT3+ORB_STATUS.changes[ORB_STATUS.current_orb].split_time_period; |
62 |
|
63 |
} |
64 |
else { //we are done with these pulses |
65 |
orb_setup_pulse(); |
66 |
} |
67 |
|
68 |
} |
69 |
|
70 |
|
71 |
//sets a channel to a value
|
72 |
void orb_set_angle(int orb, int angle) { |
73 |
uint8_t mysreg; |
74 |
|
75 |
orb=orb&0x07; //only have 8 |
76 |
angle=angle&0xff; //only accept 0-255 |
77 |
angle=255-angle; //inverse intensity |
78 |
angle=angle<<2; //scale up so that we dont run it too often |
79 |
angle+=3; //0 values dont really work |
80 |
if (ORB_STATUS.orb_angles[orb] != angle) { //if the angle has changed |
81 |
mysreg=SREG; |
82 |
cli(); //disable interrupts
|
83 |
ORB_STATUS.orb_angles[orb] = angle; //update angle
|
84 |
ORB_STATUS.new_angles = 1;
|
85 |
SREG=mysreg; //put interrupt status back
|
86 |
} |
87 |
} |
88 |
|
89 |
|
90 |
void orb_sort(void) { |
91 |
int done = 0, i; |
92 |
|
93 |
while (! done) {
|
94 |
done = 1;
|
95 |
|
96 |
for (i = 0; i < ORB_COUNT - 1; ++i) { //loop through all |
97 |
|
98 |
//if they are out of order, swap them
|
99 |
if (ORB_STATUS.orbs[i].angle > ORB_STATUS.orbs[i+1].angle) { |
100 |
ORB_STATUS.orbs[i].angle ^= ORB_STATUS.orbs[i+1].angle;
|
101 |
ORB_STATUS.orbs[i+1].angle ^= ORB_STATUS.orbs[i].angle;
|
102 |
ORB_STATUS.orbs[i].angle ^= ORB_STATUS.orbs[i+1].angle;
|
103 |
|
104 |
ORB_STATUS.orbs[i].num ^= ORB_STATUS.orbs[i+1].num;
|
105 |
ORB_STATUS.orbs[i+1].num ^= ORB_STATUS.orbs[i].num;
|
106 |
ORB_STATUS.orbs[i].num ^= ORB_STATUS.orbs[i+1].num;
|
107 |
|
108 |
done = 0;
|
109 |
} |
110 |
} |
111 |
} |
112 |
} |
113 |
|
114 |
//calculate the split times
|
115 |
void orb_setup_pulse(void) { |
116 |
int i;
|
117 |
uint16_t my_port; |
118 |
uint16_t sum = 0;
|
119 |
uint16_t split_time; |
120 |
|
121 |
my_port = 0xff; //all on |
122 |
|
123 |
if (ORB_STATUS.new_angles) {
|
124 |
|
125 |
ORB_STATUS.change_count = 0;
|
126 |
for (i = 0; i < ORB_COUNT; ++i) { //get the new values |
127 |
ORB_STATUS.orbs[i].angle = ORB_STATUS.orb_angles[ORB_STATUS.orbs[i].num]; |
128 |
} |
129 |
|
130 |
orb_sort(); //sort them
|
131 |
ORB_STATUS.new_angles = 0;
|
132 |
|
133 |
for (i = 0; i < ORB_COUNT; ++i) { //calculate split times |
134 |
split_time = ORB_STATUS.orbs[i].angle - sum; |
135 |
my_port &= ~_BV(ORB_STATUS.orbs[i].num); |
136 |
|
137 |
for (; i < ORB_COUNT - 1 && ORB_STATUS.orbs[i].angle == ORB_STATUS.orbs[i+1].angle; ++i) { |
138 |
my_port &= ~_BV(ORB_STATUS.orbs[i+1].num); //look for doups |
139 |
} |
140 |
|
141 |
ORB_STATUS.changes[ORB_STATUS.change_count].port_val = my_port; //which pins are low
|
142 |
ORB_STATUS.changes[ORB_STATUS.change_count].split_time_period = split_time; |
143 |
|
144 |
++ORB_STATUS.change_count; |
145 |
|
146 |
sum += split_time; |
147 |
} |
148 |
|
149 |
ORB_STATUS.changes[ORB_STATUS.change_count].port_val = my_port; |
150 |
ORB_STATUS.changes[ORB_STATUS.change_count].split_time_period = ORB_RESET - sum; //get a constant period
|
151 |
|
152 |
++ORB_STATUS.change_count; |
153 |
|
154 |
} |
155 |
|
156 |
|
157 |
|
158 |
ORB_STATUS.current_orb = 0;
|
159 |
|
160 |
ORBPORT |= ORBMASK; //start with all high
|
161 |
OCR3C = TCNT3 + ORB_STATUS.changes[0].split_time_period; //wait for first split |
162 |
|
163 |
} |
164 |
|
165 |
|
166 |
//Initializes the PWM for Orb control. This must be called before the Orbs are used for them to function.
|
167 |
void orb_init()
|
168 |
{ |
169 |
int i;
|
170 |
uint8_t mysreg; |
171 |
|
172 |
ORBDDR |= ORBMASK; //all outputs
|
173 |
|
174 |
mysreg=SREG; |
175 |
cli(); //turn off interrupts for now
|
176 |
|
177 |
//init everything
|
178 |
|
179 |
for (i = 0; i < ORB_COUNT; ++i) { |
180 |
ORB_STATUS.orbs[i].num = i; |
181 |
ORB_STATUS.orbs[i].angle = 1023; //127 is a pretty stupid start angle, but oh well |
182 |
ORB_STATUS.orb_angles[i] = 1023;
|
183 |
} |
184 |
|
185 |
ORB_STATUS.new_angles = 1;
|
186 |
ORB_STATUS.change_count = 0;
|
187 |
|
188 |
//init timer3
|
189 |
TCCR3A = 0;
|
190 |
TCCR3B = _BV(CS31); //prescale = 8
|
191 |
TCCR3C = 0;
|
192 |
ETIMSK |= _BV(OCIE3C); //turn on oc3c interrupt
|
193 |
OCR3C = TCNT3+ORB_RESET; |
194 |
|
195 |
SREG=mysreg; |
196 |
|
197 |
} |
198 |
|
199 |
//Set both Orbs (1 and 2) to the same color as specified
|
200 |
void orb_set(unsigned char red_led, unsigned char green_led, unsigned char blue_led) { |
201 |
orb1_set(red_led,green_led,blue_led); |
202 |
orb2_set(red_led,green_led,blue_led); |
203 |
|
204 |
} |
205 |
|
206 |
//Sets Orb1 to the specified color
|
207 |
void orb1_set(unsigned char red_led, unsigned char green_led, unsigned char blue_led) { |
208 |
orb_set_angle(0,red_led);
|
209 |
orb_set_angle(1,green_led);
|
210 |
orb_set_angle(2,blue_led);
|
211 |
} |
212 |
|
213 |
//Sets Orb2 to the specified color
|
214 |
void orb2_set(unsigned char red_led, unsigned char green_led, unsigned char blue_led) { |
215 |
orb_set_angle(4,red_led);
|
216 |
orb_set_angle(5,green_led);
|
217 |
orb_set_angle(6,blue_led);
|
218 |
} |
219 |
|
220 |
|
221 |
//Sets both orbs to the same color using the color defines
|
222 |
void orb_set_color(int col) |
223 |
{ |
224 |
int red, green, blue;
|
225 |
|
226 |
red = ((col & 0xE0) >> 5) * 36; |
227 |
green = ((col & 0x1C) >> 2) * 36; |
228 |
blue = (col & 0x03) * 85; |
229 |
|
230 |
orb_set(red, green, blue); |
231 |
} |
232 |
//Sets both orbs to the same color using the color defines
|
233 |
void orb1_set_color(int col) |
234 |
{ |
235 |
int red, green, blue;
|
236 |
|
237 |
red = ((col & 0xE0) >> 5) * 36; |
238 |
green = ((col & 0x1C) >> 2) * 36; |
239 |
blue = (col & 0x03) * 85; |
240 |
|
241 |
orb1_set(red, green, blue); |
242 |
} |
243 |
//Sets both orbs to the same color using the color defines
|
244 |
void orb2_set_color(int col) |
245 |
{ |
246 |
int red, green, blue;
|
247 |
|
248 |
red = ((col & 0xE0) >> 5) * 36; |
249 |
green = ((col & 0x1C) >> 2) * 36; |
250 |
blue = (col & 0x03) * 85; |
251 |
|
252 |
orb2_set(red, green, blue); |
253 |
} |
254 |
|
255 |
//DOES THIS WORK?
|
256 |
// Disables the timer1 interrupt, disabling the Orb's color fading capabilities
|
257 |
// You can still turn the red, green, and blue leds on and off with set_orb_dio
|
258 |
/* If we use the PWM for anything else besides the ORB, this implementation needs to be done better */
|
259 |
void orb_disable()
|
260 |
{ |
261 |
TCCR3B &= 0; //Turn off everything |
262 |
ORB_PORT |= _BV(ORB1_RED); |
263 |
ORB_PORT |= _BV(ORB1_GREEN); |
264 |
ORB_PORT |= _BV(ORB1_BLUE); |
265 |
ORB_PORT |= _BV(ORB2_RED); |
266 |
ORB_PORT |= _BV(ORB2_GREEN); |
267 |
ORB_PORT |= _BV(ORB2_BLUE); |
268 |
} |
269 |
|
270 |
//DOES THIS WORK?
|
271 |
// Enables the timer1 interrupt, enabling the Orb's color fading capabilities
|
272 |
void orb_enable()
|
273 |
{ |
274 |
// TCCR0 |= _BV(COM01) | _BV(COM00) | _BV(WGM00) | _BV(CS01); //Toggle OC Pin on match, FAST PWM Mode, clock/8
|
275 |
TCCR3B =_BV(CS31); |
276 |
|
277 |
} |
278 |
|
279 |
void orb_tester()
|
280 |
{ |
281 |
orb_init(); |
282 |
lcd_init(); |
283 |
sei(); |
284 |
|
285 |
while(1) |
286 |
{ |
287 |
orb_set_color(RED); |
288 |
//orb_set1(128,128,128);
|
289 |
//orb_set2(1,56,255);
|
290 |
delay_ms(100);
|
291 |
for (int i = 0; i < ORB_COUNT; i++) { |
292 |
lcd_putint(ORB_STATUS.changes[ORB_STATUS.current_orb].split_time_period>>3);
|
293 |
lcd_putchar(' ');
|
294 |
} |
295 |
|
296 |
delay_ms(2000);
|
297 |
lcd_clear_screen(); |
298 |
orb_set_color(GREEN); |
299 |
//orb_set1(128,128,128);
|
300 |
//orb_set2(128,128,128);
|
301 |
delay_ms(100);
|
302 |
for (int i = 0; i < ORB_COUNT; i++) { |
303 |
lcd_putint(ORB_STATUS.changes[ORB_STATUS.current_orb].split_time_period>>3);
|
304 |
lcd_putchar(' ');
|
305 |
} |
306 |
|
307 |
delay_ms(2000);
|
308 |
lcd_clear_screen(); |
309 |
} |
310 |
} |