## Revision 1345

Final spline code for master/slave

updated outdated libdragonfly and libwireless

View differences:

lights.c
1
```/**
```
2
``` * Copyright (c) 2007 Colony Project
```
3
``` *
```
4
``` * Permission is hereby granted, free of charge, to any person
```
5
``` * obtaining a copy of this software and associated documentation
```
6
``` * files (the "Software"), to deal in the Software without
```
7
``` * restriction, including without limitation the rights to use,
```
8
``` * copy, modify, merge, publish, distribute, sublicense, and/or sell
```
9
``` * copies of the Software, and to permit persons to whom the
```
10
``` * Software is furnished to do so, subject to the following
```
11
``` * conditions:
```
12
``` *
```
13
``` * The above copyright notice and this permission notice shall be
```
14
``` * included in all copies or substantial portions of the Software.
```
15
``` *
```
16
``` * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
```
17
``` * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
```
18
``` * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
```
19
``` * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
```
20
``` * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
```
21
``` * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
```
22
``` * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
```
23
``` * OTHER DEALINGS IN THE SOFTWARE.
```
24
``` **/
```
25

26
```/**
```
27
``` * @file ligths.c
```
28
``` * @brief Orbs
```
29
``` *
```
30
``` * Implemenation for the orbs (tri-colored LEDs)
```
31
``` *
```
32
``` * @author Colony Project, CMU Robotics Club
```
33
``` * @bug Unfinished
```
34
``` **/
```
35

1 36
```/*
```
2 37
```lights.c
```
3
```Controls orb1 and orb2. Also contains the framework for a software PWM that may be used for servos in the future.
```
38
```Controls orb1 and orb2. Can be extended for a software PWM that may be used for servos in the future (although maybe
```
39
```using a different timer might be preferable).
```
4 40

5 41
```author: CMU Robotics Club, Colony Project
```
6 42

7 43
```Change Log:
```
8
```2.4.07 - Aaron
```
9
```	Revamped orb code so it works.  Need to check interaction with rtc, and tweak some colors.
```
44
```3/31/2009 - Martin
```
45
```    Rewritten from scratch. Fixes code duplication, long ISRs, bugs, unnecessary synchronized code, memory waste
```
46
```*/
```
10 47

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 48

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 49

20
```*/
```
50
```/*
```
51
``` * Test cases:
```
52
``` *   - The following code has to work without flickering:
```
53
``` *     orb_init_pwm(); while(1) { orbs_set(1,1,1,254,254,254); }
```
54
``` */
```
21 55

56
```/*
```
57
``` * Possible optimizations:
```
58
``` *   - Use pointers instead of indicies for current_pwm_channel
```
59
``` *   - Optimize output_compare()
```
60
``` *   - Use a different sorting algorithm (see sort_orbs_buffer for further comments on this issue)
```
61
``` *   - Use pointers in fill_orbs_buffer
```
62
``` *   - Optimized orb_set (use the knowledge that there are only 3 distinct values, don't use a loop but unroll the
```
63
``` *     sorting, which is no problem for 3 values)
```
64
``` *   - Use a lower update frequency. The next higher prescaler value leads to a frequency of 30Hz which is too low (the
```
65
``` *     orbs are flickering). So the timer would have to be reloaded manually after 127 to generate 60Hz. This would
```
66
``` *     decrease the resolution from 8 to 7 bit, but 128 steps should still be enough.
```
67
``` *   - On setting the orbs, combine channels with the same time. This would reduce the all-values-equal OC interrupt
```
68
``` *     (30us) to the time of one regular OC interrupt (6us/10us). Also, it would reduce the total cpu usage whenever
```
69
``` *     some of the values are equal.
```
70
``` *
```
71
``` * When code is changed, the performance measurements above should be redone.
```
72
``` */
```
73

74

75

22 76
```#include "lights.h"
```
23
```#include "dragonfly_lib.h"
```
77

24 78
```#include <avr/interrupt.h>
```
25 79

80
```#include "dragonfly_lib.h"
```
26 81

27
```#define ORB_RESET 1025
```
28 82

83
```// ***************
```
84
```// ** Constants **
```
85
```// ***************
```
86

87
```#define NUM_ORBS 2   // Number or orbs
```
88
```#define NUM_COLORS 3 // Number of colors per orb
```
89
```#define num_pwm_channels NUM_ORBS*NUM_COLORS
```
90

91

92
```// *********
```
93
```// ** I/O **
```
94
```// *********
```
95

96
```// Orb port
```
29 97
```#define ORBPORT PORTC
```
98
```#define ORBDDR  DDRC
```
30 99

31
```#define ORBDDR DDRC
```
100
```// Orb pins
```
101
```#define ORB1_RED   0
```
102
```#define ORB1_GREEN 1
```
103
```#define ORB1_BLUE  2
```
104
```#define ORB2_RED   4
```
105
```#define ORB2_GREEN 5
```
106
```#define ORB2_BLUE  6
```
32 107

33
```#define ORBMASK 0x77
```
34 108

109
```// ***************
```
110
```// ** Debugging **
```
111
```// ***************
```
35 112

36
```/***** Port and Pin Definitions ****/
```
113
```//#define LIGHTS_DEBUG
```
114
```#undef LIGHTS_DEBUG
```
37 115

38
```//Orb Ports and Registers
```
39
```#define ORB_PORT        PORTC
```
40
```#define ORB_DDR         DDRC
```
116
```#define LIGHTS_DEBUG_INIT                             DDRF=6;
```
117
```#define LIGHTS_DEBUG_OVERFLOW_INTERRUPT_START         PORTF|=4;
```
118
```#define LIGHTS_DEBUG_OVERFLOW_INTERRUPT_END           PORTF&=~4;
```
119
```#define LIGHTS_DEBUG_OUTPUT_COMPARE_INTERRUPT_START   PORTF|=2;
```
120
```#define LIGHTS_DEBUG_OUTPUT_COMPARE_INTERRUPT_END     PORTF&=~2;
```
121
```#define LIGHTS_DEBUG_APPLY_START                      //PORTF|=2;
```
122
```#define LIGHTS_DEBUG_APPLY_END                        //PORTF&=~2;
```
41 123

42
```//Orb Pins
```
43
```#define ORB1_RED        0x00
```
44
```#define ORB1_GREEN      0x01
```
45
```#define ORB1_BLUE       0x02
```
46
```#define ORB2_RED        0x04
```
47
```#define ORB2_GREEN      0x05
```
48
```#define ORB2_BLUE       0x06
```
49 124

125
```// ***********
```
126
```// ** Masks **
```
127
```// ***********
```
50 128

51
```#define ORB_COUNT 8	//please dont change this, or bad things might happen
```
129
```// Some useful bit masks. All of them are are calculated from the I/O definitions above. The calculations should be done
```
130
```// at compile time (even if they are not, they are only executed once at startup).
```
52 131

53
```// an orb node
```
54
```struct ORB_NODE {
```
55
```   uint8_t num;
```
56
```   uint16_t angle;
```
132
```// Masks for the individual LEDs
```
133
```#define orb1_red_mask    _BV (ORB1_RED  )
```
134
```#define orb1_green_mask  _BV (ORB1_GREEN)
```
135
```#define orb1_blue_mask   _BV (ORB1_BLUE )
```
136
```#define orb2_red_mask    _BV (ORB2_RED  )
```
137
```#define orb2_green_mask  _BV (ORB2_GREEN)
```
138
```#define orb2_blue_mask   _BV (ORB2_BLUE )
```
139

140
```// Mask for all LEDs
```
141
```#define all_orbs_mask \
```
142
```    orb1_red_mask | orb1_green_mask | orb1_blue_mask | \
```
143
```    orb2_red_mask | orb2_green_mask | orb2_blue_mask;
```
144

145
```// Mask for the individual LEDs, organized as an array for programmatic access. The layout of this array is
```
146
```// orb_mask[orb_num, color_num]
```
147
```const uint8_t orb_mask[NUM_ORBS][NUM_COLORS]={
```
148
```    { orb1_red_mask, orb1_green_mask, orb1_blue_mask },
```
149
```    { orb2_red_mask, orb2_green_mask, orb2_blue_mask }
```
57 150
```};
```
58 151

59
```//the change in an orb
```
60
```struct ORB_CHANGE {
```
61
```   uint16_t port_val;
```
62
```   uint16_t split_time_period;
```
152
```// ***********
```
153
```// ** Types **
```
154
```// ***********
```
155

156
```struct pwm_channel_t { // 2 bytes
```
157
```    uint8_t time;
```
158
```    uint8_t mask;
```
63 159
```};
```
64 160

65
```// the status of an orb
```
66
```struct ORB_STATUS_STRUCT {
```
67
```   struct ORB_NODE orbs[ORB_COUNT];
```
68
```   uint16_t orb_angles[ORB_COUNT];
```
69
```   struct ORB_CHANGE changes[ORB_COUNT+1];
```
70
```   uint8_t change_count;
```
71
```   uint8_t new_angles;
```
72
```   uint8_t current_orb;
```
161
```struct pwm_t { // 13 bytes
```
162
```    uint8_t init_mask;
```
163
```    struct pwm_channel_t channel[num_pwm_channels];
```
164
```};
```
73 165

74
```} ORB_STATUS;
```
75 166

76
```void orb_sort(void);
```
77
```void orb_setup_pulse(void);
```
167
```// ***************
```
168
```// ** Variables **
```
169
```// ***************
```
78 170

79
```SIGNAL (SIG_OUTPUT_COMPARE3C){
```
171
```// Whether to use PWM (true) or binary (false) orb mode. Not volatile because it's only read once per function.
```
172
```bool enable_orb_pwm=true;
```
80 173

81
```		//pull the correct ones down
```
82
```      ORBPORT &= (~ORBMASK)|(ORB_STATUS.changes[ORB_STATUS.current_orb].port_val);
```
174
```// The PWM channels and the buffer pointers. This data structure is triple buffered, see above for the reasons. Not
```
175
```// volatile because they are not modified asynchronously (the read buffer is never written to asynchronously).
```
176
```struct pwm_t pwm_buffer;
```
83 177

84
```      ++ORB_STATUS.current_orb; //now look at next orb transition
```
178
```// The front buffer the ISR reads from. Other threads may not touch this pointer or the buffer it points to. Not
```
179
```// volatile because it may only be modified by the ISR.
```
180
```struct pwm_t *pwm_read_buffer =&pwm_buffer;
```
85 181

86
```      if (ORB_STATUS.current_orb < ORB_STATUS.change_count) { //if it isnt the end...
```
87
```
```
88
```			//setup timer for next pull down
```
89
```         OCR3C = TCNT3+ORB_STATUS.changes[ORB_STATUS.current_orb].split_time_period;
```
90
```
```
91
```     }
```
92
```      else { //we are done with these pulses
```
93
```		orb_setup_pulse();
```
94
```      }
```
182
```// The back buffer we can write to. The ISR may not touch this pointer or the buffer it points to. Not volatile because
```
183
```// it may only be modified by the caller.
```
184
```struct pwm_t *pwm_write_buffer=&pwm_buffer;
```
95 185

96
```}
```
186
```// The middle buffer to flip the write or read buffer with. Not volatile because it is only read once per function.
```
187
```struct pwm_t *pwm_free_buffer =&pwm_buffer;
```
97 188

189
```// Whether to perform a page flip on the beginning of the next PWM cycle. Not volatile because it is only read once
```
190
```// per function.
```
191
```bool pwm_page_flip=false; // Whether to do a page flip on the next overflow
```
98 192

99
```//sets a channel to a value
```
100
```void orb_set_angle(int orb, int angle) {
```
101
```	uint8_t mysreg;
```
193
```// The orb value array. Orb values are written here to be sorted into pwm_channels. Not volatile because all accesses
```
194
```// are from guarded (thread safe) functions.
```
195
```uint8_t orb_values[NUM_ORBS][NUM_COLORS];
```
196

197

198
```// ****************
```
199
```// ** Timer ISRs **
```
200
```// ****************
```
201

202
```// Not volatile because it is only accessed in the interrupt handler.
```
203
```uint8_t current_pwm_channel=0;
```
204

205

206
```static void output_compare (void) {
```
207
```	// This function is called when an output compare condition may have occured.
```
102 208
```
```
103
```	orb=orb&0x07; //only have 8
```
104
```	angle=angle&0xff; //only accept 0-255
```
105
```	angle=255-angle; //inverse intensity
```
106
```	angle=angle<<2; //scale up so that we dont run it too often
```
107
```	angle+=3; //0 values dont really work
```
108
```   if (ORB_STATUS.orb_angles[orb] != angle) { //if the angle has changed
```
109
```	  mysreg=SREG;
```
110
```	  cli(); //disable interrupts
```
111
```      ORB_STATUS.orb_angles[orb] = angle; //update angle
```
112
```      ORB_STATUS.new_angles = 1;
```
113
```	  SREG=mysreg; //put interrupt status back
```
114
```   }
```
209
```    // If the OC interrupt is executed without delay, TCNT0==time+1 (where time==OCR0), because the interrupt flag is
```
210
```	// queued at the next timer clock cycle after an output compare.
```
211

212
```    // What may happen here is that the interrupt is delayed for more than one timer clock cycle (33 us). In that case,
```
213
```	// the timer has already counted on and TCNT0 is bigger than current_channel_timer. Also, while during the ISR no
```
214
```	// other interrupts will occur, the timer may still count on. Thus, we have to check the following channel as well.
```
215

216
```	// Some optimization is possible in this function.
```
217

218
```    while (1) {
```
219
```		// The timer value at which the output compare interrupt should occur (one timer clock cycle after the output
```
220
```		// compare condition is detected).
```
221
```		uint8_t current_channel_time=pwm_read_buffer->channel[current_pwm_channel].time+1;
```
222
```
```
223
```		// If the counter is not at this time yet, we don't have to do anything right now.
```
224
```		if (current_channel_time>TCNT0) return;
```
225
```
```
226
```		// We have an output compare condition for the current channel.
```
227
```
```
228
```        // Turn the current channel off
```
229
```        ORBPORT|=pwm_read_buffer->channel[current_pwm_channel].mask;
```
230

231
```		// If this was the last channel, exit
```
232
```		if (current_pwm_channel==num_pwm_channels-1) return;
```
233

234
```        // Increment the channel index
```
235
```        current_pwm_channel++;
```
236

237
```        // There is a next channel, load its OCR value
```
238
```        if (pwm_read_buffer->channel[current_pwm_channel].time<255)
```
239
```            OCR0=pwm_read_buffer->channel[current_pwm_channel].time;
```
240
```    }
```
115 241
```}
```
116 242

243
```SIGNAL (SIG_OVERFLOW0) {
```
244
```#ifdef LIGHTS_DEBUG
```
245
```    LIGHTS_DEBUG_OVERFLOW_INTERRUPT_START
```
246
```#endif
```
117 247

118
```void orb_sort(void) {
```
119
```   int done = 0, i;
```
120
```
```
121
```   while (! done) {
```
122
```      done = 1;
```
248
```    if (pwm_page_flip) {
```
249
```        // Flip the read buffer with the free buffer. We are in an ISR (and we didn't re-enable interrupts), so we don't
```
250
```        // have to synchronize explicitly.
```
251
```        struct pwm_t *temp = pwm_read_buffer;
```
252
```        pwm_read_buffer    = pwm_free_buffer;
```
253
```        pwm_free_buffer    = temp;
```
254
```        pwm_page_flip=false;
```
255
```    }
```
123 256

124
```      for (i = 0; i < ORB_COUNT - 1; ++i) {  //loop through all
```
125
```
```
126
```			//if they are out of order, swap them
```
127
```         if (ORB_STATUS.orbs[i].angle > ORB_STATUS.orbs[i+1].angle) {
```
128
```            ORB_STATUS.orbs[i].angle ^= ORB_STATUS.orbs[i+1].angle;
```
129
```            ORB_STATUS.orbs[i+1].angle ^= ORB_STATUS.orbs[i].angle;
```
130
```            ORB_STATUS.orbs[i].angle ^= ORB_STATUS.orbs[i+1].angle;
```
257
```    // Turn only the appropriate PWM channels on. Do this directly on the orb port because at this point all orbs should
```
258
```    // be off anyway.
```
259
```    ORBPORT|=all_orbs_mask;
```
260
```    ORBPORT&=pwm_read_buffer->init_mask;
```
261
```
```
262
```    // Start at the first channel
```
263
```    current_pwm_channel=0;
```
264
```
```
265
```    // Load the first OCR
```
266
```    OCR0=pwm_read_buffer->channel[current_pwm_channel].time;
```
131 267

132
```            ORB_STATUS.orbs[i].num ^= ORB_STATUS.orbs[i+1].num;
```
133
```            ORB_STATUS.orbs[i+1].num ^= ORB_STATUS.orbs[i].num;
```
134
```            ORB_STATUS.orbs[i].num ^= ORB_STATUS.orbs[i+1].num;
```
268
```	// If this interrupt was delayed, we might already have an output compare condition.
```
269
```	output_compare ();
```
135 270

136
```            done = 0;
```
137
```         }
```
138
```      }
```
139
```   }
```
271
```#ifdef LIGHTS_DEBUG
```
272
```    LIGHTS_DEBUG_OVERFLOW_INTERRUPT_END
```
273
```#endif
```
140 274
```}
```
141 275

142
```//calculate the split times
```
143
```void orb_setup_pulse(void) {
```
144
```   int i;
```
145
```   uint16_t my_port;
```
146
```   uint16_t sum = 0;
```
147
```   uint16_t split_time;
```
276
```SIGNAL(SIG_OUTPUT_COMPARE0) {
```
277
```#ifdef LIGHTS_DEBUG
```
278
```    LIGHTS_DEBUG_OUTPUT_COMPARE_INTERRUPT_START
```
279
```#endif
```
148 280

149
```   my_port = 0xff; //all on
```
281
```	// We have an output compare condition.
```
282
```	output_compare ();
```
283
```
```
284
```#ifdef LIGHTS_DEBUG
```
285
```    LIGHTS_DEBUG_OUTPUT_COMPARE_INTERRUPT_END
```
286
```#endif
```
287
```}
```
150 288

151
```   if (ORB_STATUS.new_angles) {
```
152 289

153
```      ORB_STATUS.change_count = 0;
```
154
```	  for (i = 0; i < ORB_COUNT; ++i) { //get the new values
```
155
```         ORB_STATUS.orbs[i].angle = ORB_STATUS.orb_angles[ORB_STATUS.orbs[i].num];
```
156
```      }
```
157 290

158
```      orb_sort(); //sort them
```
159
```      ORB_STATUS.new_angles = 0;
```
291
```// ************************************
```
292
```// ** Internal orb setting functions **
```
293
```// ************************************
```
160 294

161
```      for (i = 0; i < ORB_COUNT; ++i) { //calculate split times
```
162
```         split_time = ORB_STATUS.orbs[i].angle - sum;
```
163
```         my_port &= ~_BV(ORB_STATUS.orbs[i].num);
```
164
```
```
165
```         for (; i < ORB_COUNT - 1 && ORB_STATUS.orbs[i].angle == ORB_STATUS.orbs[i+1].angle; ++i) {
```
166
```            my_port &= ~_BV(ORB_STATUS.orbs[i+1].num); //look for doups
```
167
```         }
```
168
```
```
169
```         ORB_STATUS.changes[ORB_STATUS.change_count].port_val = my_port; //which pins are low
```
170
```         ORB_STATUS.changes[ORB_STATUS.change_count].split_time_period = split_time;
```
171
```
```
172
```		 ++ORB_STATUS.change_count;
```
173
```
```
174
```         sum += split_time;
```
175
```      }
```
295
```static void sort_orbs_buffer (void) {
```
296
```    // This function applies a bubble sort to sort the elements of the pwm_write_buffer->channel array by the time
```
297
```    // field.
```
298
```    // This implementation is heavily optimized. Note that due to the low (and constant) number of elements to be
```
299
```    // sorted, the runtime complexity (O(n^2) for bubble sort) is not relevant here. In fact, a more advanced algorithm
```
300
```    // like quick sort or merge sort might even be slower due to higher overhead.
```
301
```    // That said, it is possible that selection sort (which is also in O(n^2)) would be faster that bubble sort because
```
302
```    // it only has to do a maximum of (n-1) swapping steps (as opposed to n*(n-1)/2 for bubble sort). However, the check
```
303
```    // if the elements are already in the correct order would either have to be left out (doing the full search every
```
304
```    // time, even if the array is already sorted) or done explicitly, so selection sort might actually be slower than
```
305
```    // bubble sort, especially if the array is already sorted or almost sorted.
```
306
```
```
307
```    // This implementation uses macros to make the algorithm more clear because the loop is rolled out and the function
```
308
```    // would become quite long without macros.
```
309
```
```
310
```    // Macro to swap two values of any type. Requires a variable of the appropriate type called swap_temp.
```
311
```    #define swap(a,b) { swap_temp=a; a=b; b=swap_temp; }
```
176 312

177
```      ORB_STATUS.changes[ORB_STATUS.change_count].port_val = my_port;
```
178
```      ORB_STATUS.changes[ORB_STATUS.change_count].split_time_period = ORB_RESET - sum; //get a constant period
```
313
```    // Macro to do one bubble sorting step (compare & swap)
```
314
```    #define bubble \
```
315
```        if(a->time > b->time) \
```
316
```        { \
```
317
```            swap (a->time, b->time); \
```
318
```            swap (a->mask, b->mask); \
```
319
```            done=false; \
```
320
```        }
```
321
```
```
322
```    // Macro to move to the next bubble sort pair
```
323
```    #define next { a++; b++; }
```
179 324

180
```      ++ORB_STATUS.change_count;
```
325
```    // Whether no change was made during the last run, which means that all values are already in correct order.
```
326
```    bool done;
```
327
```
```
328
```    // A temporary variable for swapping.
```
329
```    uint8_t swap_temp;
```
330
```
```
331
```    // Precompute the first PWM channel (tested faster).
```
332
```    struct pwm_channel_t *first=&(pwm_write_buffer->channel);
```
181 333

182
```   }
```
334
```    // Pointers to the two PWM channels under inspection
```
335
```    struct pwm_channel_t *a, *b;
```
183 336

337
```    // The actual sorting
```
338
```    a=first; b=a+1; done=true;
```
339
```    bubble next bubble next bubble next bubble next bubble
```
340
```    if (done) return;
```
184 341

342
```    a=first; b=a+1; done=true;
```
343
```    bubble next bubble next bubble next bubble
```
344
```    if (done) return;
```
185 345

186
```   ORB_STATUS.current_orb = 0;
```
346
```    a=first; b=a+1; done=true;
```
347
```    bubble next bubble next bubble
```
348
```    if (done) return;
```
187 349

188
```    ORBPORT |= ORBMASK; //start with all high
```
189
```	OCR3C = TCNT3 + ORB_STATUS.changes.split_time_period; //wait for first split
```
350
```    a=first; b=a+1; done=true;
```
351
```    bubble next bubble
```
352
```    if (done) return;
```
190 353

354
```    a=first; b=a+1; done=true;
```
355
```    bubble
```
356
```    if (done) return;
```
357

358
```    // Undefine the macros so they do not disturb some other function.
```
359
```    #undef next
```
360
```    #undef bubble
```
361
```    #undef swap
```
191 362
```}
```
192 363

193
```/**
```
194
``` * @defgroup orbs Orbs
```
195
``` * @brief Functions for controlling the color of the orbs.
```
196
``` *
```
197
``` * Functions for controlling the color and lighting of the orbs.
```
198
``` *
```
199
``` * @{
```
200
``` **/
```
364
```static void fill_orbs_buffer (void) {
```
365
```    // We do not use a loop here because it introduces 27us overhead, which is quite much, given the total time for
```
366
```    // optimized copying and sorting of 34us (elements already in correct order) to 71 us (elements in reverse order).
```
367
```
```
368
```    #define copy_value(orb, color) \
```
369
```        index=NUM_COLORS*orb+color; \
```
370
```        time=orb_values[orb][color]; \
```
371
```        mask=orb_mask[orb][color]; \
```
372
```        \
```
373
```        pwm_write_buffer->channel[index].time=time-1; \
```
374
```        pwm_write_buffer->channel[index].mask=mask; \
```
375
```        \
```
376
```        if (time!=0) \
```
377
```            pwm_write_buffer->init_mask &= ~mask; \
```
201 378

202
```/**
```
203
``` * Initializes the PWM for Orb control. This must be called before
```
204
``` * the orbs are used for them to function.
```
205
``` **/
```
206
```void orb_init()
```
207
```{
```
208
```   int i;
```
209
```   uint8_t mysreg;
```
210
```
```
211
```   ORBDDR |= ORBMASK;	//all outputs
```
212
```
```
213
```	mysreg=SREG;
```
214
```	cli(); //turn off interrupts for now
```
379
```    uint8_t index, time, mask;
```
380
```    copy_value(0,0); copy_value(0,1); copy_value(0,2);
```
381
```    copy_value(1,0); copy_value(1,1); copy_value(1,2);
```
382
```
```
383
```    #undef copy_value
```
384
```}
```
215 385

216
```	//init everything
```
386
```static void apply_orbs (void) {
```
387
```	/*
```
388
```	 * Some timing tests: Time for apply_orbs with interrupts disabled, in us:
```
389
```     *             Values in:     Correct order      Reverse order
```
390
```     * Naive bubble sort:         148                217
```
391
```     * Aborting bubble sort:       71                232
```
392
```     * Only count to top:          73                189
```
393
```     *
```
394
```     * Loops rolled out:           61                120
```
395
```     * Using pointers:             62                 98
```
396
```     * Copy loop also rolled out:  35                 72
```
397
```	 *
```
398
```	 * Note that rolling out both loops and using pointers saves 52%/62% of time! 27us were spent on loop overhead,
```
399
```	 * which is quite much, considering an optimized total time for copying and sorting or 35us.
```
400
```	 */
```
217 401

218
```   for (i = 0; i < ORB_COUNT; ++i) {
```
219
```      ORB_STATUS.orbs[i].num = i;
```
220
```      ORB_STATUS.orbs[i].angle = 1023;	//127 is a pretty stupid start angle, but oh well
```
221
```      ORB_STATUS.orb_angles[i] = 1023;
```
222
```   }
```
402
```#ifdef LIGHTS_DEBUG
```
403
```    LIGHTS_DEBUG_APPLY_START
```
404
```#endif
```
223 405

224
```   ORB_STATUS.new_angles = 1;
```
225
```   ORB_STATUS.change_count = 0;
```
406
```    if (enable_orb_pwm) {
```
407
```        // PWM mode
```
408
```
```
409
```        pwm_write_buffer->init_mask=~0;
```
410
```
```
411
```        // 1. Write the orb values and corresponding masks to the pwm channels
```
412
```        // array unsorted.
```
413
```        fill_orbs_buffer ();
```
226 414

227
```	//init timer3
```
228
```	TCCR3A = 0;
```
229
```	TCCR3B = _BV(CS31); //prescale = 8
```
230
```	TCCR3C = 0;
```
231
```	ETIMSK |= _BV(OCIE3C); //turn on oc3c interrupt
```
232
```	OCR3C = TCNT3+ORB_RESET;
```
415
```        // 2. sort the buffer.
```
416
```        sort_orbs_buffer ();
```
233 417

234
```	SREG=mysreg;
```
418
```        // Flip the write buffer with the free buffer.
```
419
```        SYNC {
```
420
```            struct pwm_t *temp = pwm_write_buffer;
```
421
```            pwm_write_buffer   = pwm_free_buffer;
```
422
```            pwm_free_buffer    = temp;
```
423
```        }
```
424
```
```
425
```        // On the next overflow, flip the read buffer with the free buffer.
```
426
```        pwm_page_flip=true;
```
427
```    }
```
428
```    else {
```
429
```        // Binary mode.
```
430
```        // The outputs are inverted.
```
431
```        uint8_t on=0;
```
432
```
```
433
```        if (orb_values) on |= orb_mask;
```
434
```        if (orb_values) on |= orb_mask;
```
435
```        if (orb_values) on |= orb_mask;
```
436
```        if (orb_values) on |= orb_mask;
```
437
```        if (orb_values) on |= orb_mask;
```
438
```        if (orb_values) on |= orb_mask;
```
439
```
```
440
```        // Write the new orb states to the output port. Synchronized because it is a RMW operation.
```
441
```        SYNC {
```
442
```            uint8_t value=ORBPORT;
```
443
```            value |= all_orbs_mask; // All orbs off
```
444
```            value &= ~on; // Selected orbs on
```
445
```            ORBPORT=value;
```
446
```        }
```
447
```    }
```
448
```
```
449
```#ifdef LIGHTS_DEBUG
```
450
```    LIGHTS_DEBUG_APPLY_END
```
451
```#endif
```
235 452
```}
```
236 453

454
```static void set_orb_values (uint8_t num, uint8_t red, uint8_t green, uint8_t blue) {
```
455
```    // Write the values to the array, but do not sort them yet, as we might want to write the other orb values first so
```
456
```    // we don't have to sort twice.
```
457
```    // Any function calling this function will probably want to call apply_orbs() afterwards.
```
458
```    orb_values[num]=red;
```
459
```    orb_values[num]=green;
```
460
```    orb_values[num]=blue;
```
461
```}
```
462

463

464
```// ***********************
```
465
```// ** RGB color setting **
```
466
```// ***********************
```
467

468
```// All of these functions use set_orb_values to set the actual values, and then call apply_orbs() to apply the changes.
```
469
```// set_orb_values should be used (even though it would be faster to set the array directly) because the binary/pwm mode
```
470
```// has to be handled.
```
471
```// All of these functions must be
```
472

473
```uint8_t orb_lock=0;
```
474

237 475
```/**
```
238
``` * Set both orbs to the color specified. orb_init must
```
239
``` * be called before this function may be used.
```
476
``` * Sets the specified orb to the specified color. The orbs must be initialized before this function may be used.
```
477
``` * Note that, when setting both orbs, using orbs_set is faster then setting the orbs individually because the values are
```
478
``` * only sorted once.
```
240 479
``` *
```
241
``` * @param red_led the red component of the color
```
242
``` * @param green_led the green component of the color
```
243
``` * @param blue_led the blue component of the color
```
480
``` * @param num the number of the orb to set (0 or 1)
```
481
``` * @param red the red value for the specified orb
```
482
``` * @param green the green value for the specified orb
```
483
``` * @param blue the blue value for the specified orb
```
484
``` * @see
```
485
``` */
```
486
```void orb_n_set (uint8_t num, uint8_t red, uint8_t green, uint8_t blue) {
```
487
```	REQUIRE_LOCK_OR_RETURN(orb_lock);
```
488
```
```
489
```    set_orb_values (num, red, green, blue);
```
490
```    apply_orbs ();
```
491
```
```
492
```	RELEASE_LOCK(orb_lock);
```
493
```}
```
494

495
```/**
```
496
``` * Set orb1 to the color specified. The orbs must be initialized before this function may be used. Note that, when
```
497
``` * setting both orbs, using orbs_set is faster then setting the orbs individually because the values are only sorted
```
498
``` * once.
```
244 499
``` *
```
500
``` * @param red the red component of the color
```
501
``` * @param green the green component of the color
```
502
``` * @param blue the blue component of the color
```
503
``` *
```
245 504
``` * @see orb_init
```
246 505
``` **/
```
247
```void orb_set(unsigned char red_led, unsigned char green_led, unsigned char blue_led) {
```
248
```	orb1_set(red_led,green_led,blue_led);
```
249
```	orb2_set(red_led,green_led,blue_led);
```
506
```void orb1_set (uint8_t red, uint8_t green, uint8_t blue) {
```
507
```	REQUIRE_LOCK_OR_RETURN(orb_lock);
```
508
```
```
509
```    set_orb_values (0, red, green, blue);
```
510
```    apply_orbs ();
```
250 511

512
```	RELEASE_LOCK(orb_lock);
```
251 513
```}
```
252 514

253 515
```/**
```
254
``` * Set orb1 to the color specified. orb_init must
```
255
``` * be called before this function may be used.
```
516
``` * Set orb2 to the color specified. The orbs must be initialized before this function may be used. Note that, when
```
517
``` * setting both orbs, using orbs_set is faster then setting the orbs individually because the values are only sorted
```
518
``` * once.
```
256 519
``` *
```
257 520
``` * @param red_led the red component of the color
```
258 521
``` * @param green_led the green component of the color
```
......
260 523
``` *
```
261 524
``` * @see orb_init
```
262 525
``` **/
```
263
```void orb1_set(unsigned char red_led, unsigned char green_led, unsigned char blue_led) {
```
264
```	orb_set_angle(0,red_led);
```
265
```	orb_set_angle(1,green_led);
```
266
```	orb_set_angle(2,blue_led);
```
526
```void orb2_set (uint8_t red, uint8_t green, uint8_t blue) {
```
527
```	REQUIRE_LOCK_OR_RETURN(orb_lock);
```
528
```
```
529
```    set_orb_values (1, red, green, blue);
```
530
```    apply_orbs ();
```
531

532
```	RELEASE_LOCK(orb_lock);
```
267 533
```}
```
268 534

269 535
```/**
```
270
``` * Set orb2 to the color specified. orb_init must
```
271
``` * be called before this function may be used.
```
536
``` * Set both orbs to the color specified. The orbs must be initialized before this function may be used.
```
272 537
``` *
```
273 538
``` * @param red_led the red component of the color
```
274 539
``` * @param green_led the green component of the color
```
275 540
``` * @param blue_led the blue component of the color
```
276 541
``` *
```
277
``` * @see orb_init
```
542
``` * @see orb_init, orb1_set, orb2_set
```
278 543
``` **/
```
279
```void orb2_set(unsigned char red_led, unsigned char green_led, unsigned char blue_led) {
```
280
```	orb_set_angle(4,red_led);
```
281
```	orb_set_angle(5,green_led);
```
282
```	orb_set_angle(6,blue_led);
```
544
```void orb_set (uint8_t red, uint8_t green, uint8_t blue) {
```
545
```	REQUIRE_LOCK_OR_RETURN(orb_lock);
```
546
```
```
547
```    set_orb_values (0, red, green, blue);
```
548
```    set_orb_values (1, red, green, blue);
```
549
```    apply_orbs ();
```
550

551
```	RELEASE_LOCK(orb_lock);
```
283 552
```}
```
284 553

285 554
```/**
```
286
``` * Set both orbs to the specified color. This function
```
287
``` * is intended to be used with the predefined
```
288
``` * colors. orb_init must be called before this
```
289
``` * function may be used.
```
555
``` * Set the orbs to the respective values. The orbs must be initialized before this function may be used. Note that, when
```
556
``` * setting both orbs, this function is faster than calling orb1_set and orb2_set (or orb_n_set) because the values are
```
557
``` * only sorted once.
```
290 558
``` *
```
291
``` * @param col the color to set the orbs to
```
292
``` *
```
293
``` * @see orb_init
```
559
``` * @param red1
```
560
``` * @param green1
```
561
``` * @param blue1
```
562
``` * @param red2
```
563
``` * @param green2
```
564
``` * @param blue2
```
565
``` * @see orb1_set
```
566
``` * @see orb2_set
```
567
``` * @see orb_n_set
```
294 568
``` **/
```
295
```void orb_set_color(int col)
```
296
```{
```
297
``` int red, green, blue;
```
569
```void orbs_set (
```
570
```    uint8_t red1, uint8_t green1, uint8_t blue1,
```
571
```    uint8_t red2, uint8_t green2, uint8_t blue2) {
```
298 572

299
``` red = ((col & 0xE0) >> 5) * 36;
```
300
``` green = ((col & 0x1C) >> 2) * 36;
```
301
``` blue = (col & 0x03) * 85;
```
573
```	REQUIRE_LOCK_OR_RETURN(orb_lock);
```
302 574

303
``` orb_set(red, green, blue);
```
575
```    set_orb_values (0, red1, green1, blue1);
```
576
```    set_orb_values (1, red2, green2, blue2);
```
577
```    apply_orbs ();
```
578

579
```	RELEASE_LOCK(orb_lock);
```
304 580
```}
```
305 581

582

583
```// ******************************
```
584
```// ** Predefined color setting **
```
585
```// ******************************
```
586

587
```// This functions just call the corresponding orb*_set functions. If the orbs array is accessed in any other way, it
```
588
```// must be synchronized on orb_lock (REQUIRE_LOCK_OR_RETURN and RELEASE_LOCK)! Note that one synchronized function
```
589
```// cannot call another one with this lock implementation.
```
590

591
```// Macros for extracting a color.
```
592
```#define C_RED(col)   (((col & 0xE0) >> 5) * 36)
```
593
```#define C_GREEN(col) (((col & 0x1C) >> 2) * 36)
```
594
```#define C_BLUE(col)  (((col & 0x03)     ) * 85)
```
595

306 596
```/**
```
307
``` * Set orb1 to the specified color. This function
```
308
``` * is intended to be used with the predefined
```
309
``` * colors. orb_init must be called before this
```
310
``` * function may be used.
```
597
``` * Set the specified orb to the specified color. This function is intended to be used with the predefined colors.
```
311 598
``` *
```
599
``` * @param num the number of the orb to set (0 or 1)
```
312 600
``` * @param col the color to set the orbs to
```
601
``` **/
```
602
```void orb_n_set_color(uint8_t num, uint8_t col) {
```
603
```    orb_n_set(num, C_RED(col), C_GREEN(col), C_BLUE(col));
```
604
```}
```
605

606
```/**
```
607
``` * Set orb1 to the specified color. This function is intended to be used with the predefined colors.
```
313 608
``` *
```
314
``` * @see orb_init
```
609
``` * @param col the color to set the orbs to
```
315 610
``` **/
```
316
```void orb1_set_color(int col)
```
317
```{
```
318
``` int red, green, blue;
```
611
```void orb1_set_color(uint8_t col) {
```
612
```    orb1_set (C_RED(col), C_GREEN(col), C_BLUE(col));
```
613
```}
```
319 614

320
``` red = ((col & 0xE0) >> 5) * 36;
```
321
``` green = ((col & 0x1C) >> 2) * 36;
```
322
``` blue = (col & 0x03) * 85;
```
323

324
``` orb1_set(red, green, blue);
```
615
```/**
```
616
``` * Set orb2 to the specified color. This function is intended to be used with the predefined colors.
```
617
``` *
```
618
``` * @param col the color to set the orbs to
```
619
``` **/
```
620
```void orb2_set_color(uint8_t col) {
```
621
```    orb2_set(C_RED(col), C_GREEN(col), C_BLUE(col));
```
325 622
```}
```
326 623

327 624
```/**
```
328
``` * Set orb2 to the specified color. This function
```
329
``` * is intended to be used with the predefined
```
330
``` * colors. orb_init must be called before this
```
331
``` * function may be used.
```
625
``` * Set both orbs to the specified color. This function is intended to be used with the predefined colors.
```
332 626
``` *
```
333 627
``` * @param col the color to set the orbs to
```
628
``` **/
```
629
```void orb_set_color(uint8_t col) {
```
630
```    orb_set (C_RED(col), C_GREEN(col), C_BLUE(col));
```
631
```}
```
632

633
```/**
```
634
``` * Set the orbs to the respective color. This function is intended to be used with the predefined colors.
```
334 635
``` *
```
335
``` * @see orb_init
```
636
``` * @param col1 the color to set orb 1 to
```
637
``` * @param col2 the color to set orb 2 to
```
336 638
``` **/
```
337
```void orb2_set_color(int col)
```
338
```{
```
339
``` int red, green, blue;
```
639
```void orbs_set_color(uint8_t col1, uint8_t col2) {
```
640
```    orbs_set (C_RED(col1), C_GREEN(col1), C_BLUE(col1), C_RED(col2), C_GREEN(col2), C_BLUE(col2));
```
641
```}
```
340 642

341
``` red = ((col & 0xE0) >> 5) * 36;
```
342
``` green = ((col & 0x1C) >> 2) * 36;
```
343
``` blue = (col & 0x03) * 85;
```
643
```#undef C_BLUE
```
644
```#undef C_GREEN
```
645
```#undef C_RED2
```
344 646

345
``` orb2_set(red, green, blue);
```
647

648
```// ******************
```
649
```// ** Mode setting **
```
650
```// ******************
```
651

652
```/**
```
653
``` * Enables the orb timer. Note that you usually don't want to use this function directly. Instead, use orb_set_mode.
```
654
``` * @see orb_set_mode
```
655
``` **/
```
656
```void orb_enable_timer (void) {
```
657
```    // Use 8 bit TC0.
```
658
```    //
```
659
```    // Timer mode: We cannot use CTC mode because it can only clear on OCR0 (in contrast to the 16 bit timers which can
```
660
```    // also use the ICR for that) and OCR0 is already used for generating output compare interrupts. We also need
```
661
```    // immediate (non double buffered) update of OCR0, so the only mode left is "Normal".
```
662
```    //
```
663
```    // Note that for a timer counting from 0 to 255, there are 256 states and thus 257 output possibilities
```
664
```    // (0/256...256/256)! However, there are only 256 values in the byte used to specify the PWM value. Possible ways
```
665
```    // to deal with that:
```
666
```    //   1. use a 16 bit variable for the PWM value (memory waste, overhead)
```
667
```    //   2. use an additional flag for the 257th value (inconvenient)
```
668
```    //   3. use 1/256...256/256 (skip 0, never complete off)
```
669
```    //   4. use 0/256...256/256 (skip 256, never complete on)
```
670
```    //   5. skip a value somewhere in the middle
```
671
```    //   6. reload the timer after 254
```
672
```    // For this implementation, variant 4 was chosen.
```
673
```    //
```
674
```    // Using an 8 bit timer has the added advantage that all the comparisons are faster.
```
675
```
```
676
```    // Normal mode, Compare match output off, Prescaler
```
677
```    TCCR0=_BV(CS02) | _BV(CS01); // 256, 120 Hz
```
678

679
```    // Enable the interrupts
```
680
```    TIMSK|= _BV(OCIE0) | _BV(TOIE0);
```
346 681
```}
```
347 682

348
```//DOES THIS WORK?
```
349
```// Disables the timer1 interrupt, disabling the Orb's color fading capabilities
```
350
```// You can still turn the red, green, and blue leds on and off with set_orb_dio
```
351
```/* If we use the PWM for anything else besides the ORB, this implementation needs to be done better */
```
352 683
```/**
```
353
``` * Disables the orb color fading capabilities
```
354
``` * by disabling the timer1 interrupt.
```
355
``` *
```
356
``` * @see orb_init
```
684
``` * Disables the orb timer. Note that you usually don't want to use this function directly. Instead, use orb_set_mode.
```
685
``` * @see orb_set_mode
```
357 686
``` **/
```
358
```void orb_disable()
```
359
```{
```
360
```	TCCR3B &= 0;  	//Turn off everything
```
361
```	ORB_PORT |= _BV(ORB1_RED);
```
362
```	ORB_PORT |= _BV(ORB1_GREEN);
```
363
```	ORB_PORT |= _BV(ORB1_BLUE);
```
364
```	ORB_PORT |= _BV(ORB2_RED);
```
365
```	ORB_PORT |= _BV(ORB2_GREEN);
```
366
```	ORB_PORT |= _BV(ORB2_BLUE);
```
687
```void orb_disable_timer (void) {
```
688
```    // Disable the interrupts
```
689
```    TIMSK&=~( _BV(OCIE0) | _BV(TOIE0));
```
367 690
```}
```
368 691

369
```//DOES THIS WORK?
```
370
```// Enables the timer1 interrupt, enabling the Orb's color fading capabilities
```
692

693
```void orb_set_mode (orb_mode_t mode) {
```
694
```    // Set enable_orb_pwm to the appropriate value and disable or enable the timer.
```
695
```    if (mode==orb_mode_binary) {
```
696
```        orb_disable_timer ();
```
697

698
```        enable_orb_pwm=false;
```
699
```        apply_orbs ();
```
700
```    }
```
701
```    else { // orb_mode_pwm
```
702
```        enable_orb_pwm=true;
```
703
```        apply_orbs ();
```
704

705
```        orb_enable_timer ();
```
706
```    }
```
707
```}
```
708

709

710
```// ********************
```
711
```// ** Initialization **
```
712
```// ********************
```
713

714
```// Orb initialization code common to all modes.
```
715
```static void orb_init_common (void) {
```
716
```    // Enable the output ports and turn off the LEDs
```
717
```    ORBPORT |=  all_orbs_mask;
```
718
```    ORBDDR  |=  all_orbs_mask;
```
719

720
```    // Set all orbs to "off"
```
721
```    orb_set (0, 0, 0);
```
722
```
```
723
```#ifdef LIGHTS_DEBUG
```
724
```    LIGHTS_DEBUG_INIT
```
725
```#endif
```
726
```}
```
727

371 728
```/**
```
372
``` * Enables the orb's color fading capabilities.
```
373
``` *
```
374
``` * @see orb_init
```
729
``` * Initializes the orbs in PWM mode. One of the orb_init* functions must be called before the orbs can be used.
```
730
``` *
```
731
``` * @see orb_init_pwm
```
375 732
``` **/
```
376
```void orb_enable()
```
377
```{
```
378
```//	TCCR0 |= _BV(COM01) | _BV(COM00)  | _BV(WGM00) | _BV(CS01);	//Toggle OC Pin on match, FAST PWM Mode, clock/8
```
379
```	TCCR3B =_BV(CS31);
```
733
```void orb_init_binary (void) {
```
734
```    orb_init_common ();
```
735
```    orb_set_mode (orb_mode_binary);
```
380 736
```}
```
381 737

382
```/** @} **/ //end group
```
738
```/**
```
739
``` * Initializes the orbs in PWM mode. One of the orb_init* functions must be called before the orbs can be used.
```
740
``` *
```
741
``` * @see orb_init_binary
```
742
``` **/
```
743
```void orb_init_pwm (void) {
```
744
```    orb_init_common ();
```
745
```    orb_set_mode (orb_mode_pwm);
```
746
```}
```
383 747

748
```/**
```
749
``` * Initializes the orbs in default mode. One of the orb_init* functions must be called before the orbs can be used. Use
```
750
``` * the orb_init_binary or orb_init_pwm function if you want one specific mode.
```
751
``` *
```
752
``` * @see orb_init_pwm
```
753
``` * @see orb_init_binary
```
754
``` **/
```
755
```void orb_init () {
```
756
```    orb_init_pwm ();
```
757
```}
```

Also available in: Unified diff
{"ft_lang_label":"__label__en","ft_lang_prob":0.5306187,"math_prob":0.85023475,"size":38655,"snap":"2021-21-2021-25","text_gpt3_token_len":11682,"char_repetition_ratio":0.1349771,"word_repetition_ratio":0.10251955,"special_character_ratio":0.4446514,"punctuation_ratio":0.12406132,"nsfw_num_words":0,"has_unicode_error":false,"math_prob_llama3":0.9874118,"pos_list":[0],"im_url_duplicate_count":[null],"WARC_HEADER":"{\"WARC-Type\":\"response\",\"WARC-Date\":\"2021-05-18T16:31:43Z\",\"WARC-Record-ID\":\"<urn:uuid:89c1104f-d20c-4795-88cd-0cc5e136c2ec>\",\"Content-Length\":\"172522\",\"Content-Type\":\"application/http; msgtype=response\",\"WARC-Warcinfo-ID\":\"<urn:uuid:09665e3f-fa1d-46e5-87dd-f9bbeba1b9a5>\",\"WARC-Concurrent-To\":\"<urn:uuid:bfbf4dd7-ba5c-45c4-adc5-ee52f47354bd>\",\"WARC-IP-Address\":\"128.2.98.31\",\"WARC-Target-URI\":\"https://roboticsclub.org/redmine/projects/colony/repository/revisions/1345/diff/branches/encoders/code/projects/libdragonfly/lights.c\",\"WARC-Payload-Digest\":\"sha1:KNAK3KYX35MITLFNEWW2T7HVZFKHYSC6\",\"WARC-Block-Digest\":\"sha1:KEIWKQEEJXX6ESMFK7A2JHPLTJBMTB4P\",\"WARC-Identified-Payload-Type\":\"text/html\",\"warc_filename\":\"/cc_download/warc_2021/CC-MAIN-2021-21/CC-MAIN-2021-21_segments_1620243991288.0_warc_CC-MAIN-20210518160705-20210518190705-00271.warc.gz\"}"}