- The first thing you need to do is to get yourself ANY kind of scope. You can buy a DSO138 -- I've bought and assembled several of these to give away to friends. They are cheap and they may work for your needs. (It samples at \$1\:\textrm{MHz}\$ and only supports about \$200\:\textrm{kHz}\$, but that may be okay enough for what you have ahead.) Or you can get something a little fancier that hooks up to a PC, like a Hantek 6022BE. Or you can just buy a cheap, used oscilloscope. There are a number of sites for looking for good prices, many of which I'm sure you know about. But the point here is that if you are working on code to do something important to you like this, you really NEED to have something that allows you to observe and measure what you are producing. And your needs are pedestrian, so it does not cost you an arm and a leg. Probably less than the cost of one ESC controller. Worth doing.
- The ATmega16 has four PWMs. One PWM on each of two 8-bit timers and only two more PWMs on the 16-bit Timer1. So I get why you are trying to do this in software, using Timer1. There's no other way to go.
What I do NOT see in your code is a queue of any kind. And you need one, I think.
Here's how I'd approach this as a software design solution:
- Decide on a specific resolution for my four PWM outputs. I'm not an expert on ESC controllers and I don't know what resolution they use when interpreting the pulse width they receive. But I think I can infer from your use of 1000 and 2000 to suggest that you want precision down to the microsecond. So, in short, you can set \$1.001\:\textrm{ms}\$ and set \$1.583\:\textrm{ms}\$, but you cannot set \$1.6057\:\textrm{ms}\$.
- Once you know the required resolution, you can set up Timer1 to provide that resolution. This just sets up the timer's counter rate. (It doesn't cause any interrupts, by itself. That happens when you set up the capture/compares.) In this case, I will assume you have set up Timer1 to count at a rate of \$1\:\textrm{MHz}\$. (No, I'm not going to go research the datasheet on the ATmega16 and try to work out what you actually did do in your code above. Instead, I'm telling you what you need to do.)
- Now that the resolution is determined and Timer1 is configured properly to provide a \$1\:\textrm{MHz}\$ counter rate, you set up what is called a 'delta queue.' You can read about them in Douglas Comer's (the inventor) book on XINU, dated into the mid 1980's, if you are interested in a source of the idea. You will need four entries in the queue. One for each of your PWM output pins, individually.
- Let's call the \$20\:\textrm{ms}\$ interval thread, \$P_0\$. The other four will be \$P_1\dots P_4\$.
- You have two capture/compare registers available. It would probably be more convenient to use both. One of them gets assigned to \$P_0\$ for its regular \$20\:\textrm{ms}\$ event. Let's assign OCR1A to \$P_0\$, so set that value to 20000 and set it up for auto-reload, if possible. (If not, do it in software.) This will set up the basic \$20\:\textrm{ms}\$ timing.
- \$P_0\$ will do the following: (a) Drive all PWM outputs HIGH. (b) Insert each of \$P_1\dots P_4\$ into the queue, which by now is always empty, based upon their period values. (c) Clear the timer counter. (d) Load OCR1B with the timer value of the first entry in the delta queue and enable the OCR1B interrupt event. (e) Exit. (It may need to do something to enable the next interrupt event. But that's up to you to figure out.)
- On the OCR1B event, you drive the associated pin LOW. Remove the current entry from the queue. While there are remaining entries in the queue and while the remaining 'ticks' (delta value) are exactly zero, drive the associated pin LOW and remove the entry from the queue. Now, you are guaranteed one of two cases: (a) there are no remaining queue entries -- in this case, just return as there is nothing more to do. (b) there are remaining queue entries and the top one holds a non-zero timer value -- load this value into OCR1B and make sure it can re-interrupt, then just return.
That it. The whole process. The delta queue is set up to hold a 'tick' value, which is the count in microseconds for the next event to occur. When inserting an entry into the queue, you subtract all the 'ticks' (delta value) in prior entries before inserting. So let me demonstrate with an example to make this clear.
Suppose the timing value for \$P_1\dots P_4\$ are: 1573, 2000, 1206, and 1573. Then the queue would look like (not including the associated pin, which is also required here):
$$\begin{array}{lrcl}
& delta & OCR1B & Thread\\
1 & 1206 & 1206 & P_3 \\
2 & 367 & 1573 & P_1 \\
3 & 0 & 1573 & P_4 \\
4 & 427 & 2000 & P_2
\end{array}$$
That's listed in queue order. However, I'd create a five element array with indices from 0 to 4, with [0] assigned as the queue's \$head\$ pointer. So the above queue would like the following in array order:
$$\begin{array}{lcrc}
& next & delta & OCR1B\\
0 & 3 & n/a & n/a \\
1 & 4 & 367 & 1573 \\
2 & 0 & 427 & 2000 \\
3 & 1 & 1206 & 1206 \\
4 & 2 & 0 & 1573
\end{array}$$
When \$P_0\$ sets about inserting all four into the queue (which is empty when \$P_0\$ starts, since all of \$P_1 \dots p_4\$ have by this time all expired), that is the resulting queue when \$P_0\$ exits and returns from its interrupt event. But just before exiting, as mentioned, \$P_0\$ will load the first OCR1B value in the queue (1206 here) and place that into OCR1B.
When the OCR1B event triggers, the first entry in the queue tells the code which pin to drive LOW. Then the entry is removed from the queue. For the first OCR1B event, this removes the 1206 entry, leaving the delta value of 367 value, which is not zero. So the OCR1B value of 1573 is now loaded into OCR1B.
When the next event occurs, the OCR1B event will now drive that associated pin LOW, as well, and remove that entry from the queue. Now the next entry has a delta value of 0 in it. Because of that, it MUST mean that there is another pin to drive LOW, so the OCR1B event continues and drives that pin LOW as well and removes that entry from the queue. At this point, there is only one remaining entry in the queue, which has the delta value 427, which is also non-zero. So the code now loads the OCR1B value of 2000 into the compare register, OCR1B, and exits.
The last OCR1B event now triggers and the code drives that pin LOW, as well, and removes the entry. There are no more entries. So the code just exits. It's all done and the only thing remaining is to wait for the OCR1A event to re-occur.
That's the process to follow, I think.
Here's an example of how I might set up the queues for this, most especially illustrating how to insert into the delta queue. The function qinsert() does this job. I'd have \$P_0\$ call qinsert() for each of the four PWMs, as part of its job.
uint8_t qp[5]; /* prior in queue reference */
uint8_t qn[5]; /* next in queue reference */
uint16_t qk[5]; /* delta value */
uint16_t qv[5]; /* OCR1B value */
uint8_t qpin[5]; /* pin position 0..7 */
void qinit( void ) {
qn[0]= qp[0]= 0;
qk[0]= 0xFFFF;
return;
}
uint8_t qinsert( uint8_t node, uint16_t key ) {
uint8_t prv= 0, nxt;
uint16_t nxtkey;
qv[node]= key; /* optional, depending on overall design */
for ( nxt= qn[prv]; (nxtkey= qk[nxt]) < key; prv= nxt, nxt= qn[nxt] )
key -= nxtkey;
if ( nxt != 0 )
qk[nxt]= nxtkey - key;
qk[node]= key;
qp[node]= prv;
qn[node]= nxt;
qp[nxt]= qn[prv]= node;
return node;
}
uint8_t qunlink( uint8_t node ) {
uint8_t prv= qp[node], nxt= qn[node];
qn[prv]= nxt;
qp[nxt]= prv;
return node;
}