void setup() {
// Set up OC1A
TCCR1A = (1<<COM1A1) | (0<<COM1A0) | (1<<WGM11) | (0<<WGM10); // Set Pin Low on compare match + WGM to Fast PM (Mode 14, p145 Datasheet)
TCCR3A = (1<<COM3A1) | (0<<COM3A0) | (1<<WGM31) | (0<<WGM30); // Set Pin Low on compare match + WGM to Fast PM (Mode 14, p145 Datasheet)
TCCR4A = (1<<COM4A1) | (0<<COM4A0) | (1<<WGM41) | (0<<WGM40); // Set Pin Low on compare match + WGM to Fast PM (Mode 14, p145 Datasheet)
TCCR5A = (1<<COM5A1) | (0<<COM5A0) | (1<<WGM51) | (0<<WGM50); // Set Pin Low on compare match + WGM to Fast PM (Mode 14, p145 Datasheet)
}
void SetPWM(unsigned long frequency, uint8_t percentage, uint16_t prescaler, uint8_t channel) {
uint16_t resolution = F_CPU/((uint32_t)prescaler * frequency);
switch (channel) {
case 1:
ICR1 = resolution - 1;
OCR1A = (((uint32_t)percentage * resolution) / 100) - 1;
break;
case 2:
ICR3 = resolution - 1;
OCR3A = (((uint32_t)percentage * resolution) / 100) - 1;
break;
case 3:
ICR4 = resolution - 1;
OCR4A = (((uint32_t)percentage * resolution) / 100) - 1;
break;
case 4:
ICR5 = resolution - 1;
OCR5A = (((uint32_t)percentage * resolution) / 100) - 1;
break;
} // end switch
} // ---
void SetOutputs() {
uint16_t prescalerUsed = 1;
// Set up prescaler to accommodate mDelay size
TIMSK1 &= ~ (1 << OCIE1A); // DISABLE interrupt on compare match
TIMSK3 &= ~ (1 << OCIE3A); // DISABLE interrupt on compare match
TIMSK4 &= ~ (1 << OCIE4A); // DISABLE interrupt on compare match
TIMSK5 &= ~ (1 << OCIE5A); // DISABLE interrupt on compare match
for (uint8_t mNo=1; mNo<=4; mNo++) { // For each motor
if (mDelay[mNo-1] / 1024 < 65535) { // Max timer can hold
if (mNo == 1) TCCR1B = (0<<ICNC1) | (0<<ICES1) | (1<<WGM13) | (1<<WGM12) | (1<<CS12) | (0<<CS11) | (1<<CS10); //1024
if (mNo == 2) TCCR3B = (0<<ICNC3) | (0<<ICES3) | (1<<WGM33) | (1<<WGM32) | (1<<CS32) | (0<<CS31) | (1<<CS30); //1024
if (mNo == 3) TCCR4B = (0<<ICNC4) | (0<<ICES4) | (1<<WGM43) | (1<<WGM42) | (1<<CS42) | (0<<CS41) | (1<<CS40); //1024
if (mNo == 4) TCCR5B = (0<<ICNC5) | (0<<ICES5) | (1<<WGM53) | (1<<WGM52) | (1<<CS52) | (0<<CS51) | (1<<CS50); //1024
prescalerUsed = 1024;
}
if (mDelay[mNo-1] / 256 < 65535) {
if (mNo == 1) TCCR1B = (0<<ICNC1) | (0<<ICES1) | (1<<WGM13) | (1<<WGM12) | (1<<CS12) | (0<<CS11) | (0<<CS10); //256
if (mNo == 2) TCCR3B = (0<<ICNC3) | (0<<ICES3) | (1<<WGM33) | (1<<WGM32) | (1<<CS32) | (0<<CS31) | (0<<CS30); //256
if (mNo == 3) TCCR4B = (0<<ICNC4) | (0<<ICES4) | (1<<WGM43) | (1<<WGM42) | (1<<CS42) | (0<<CS41) | (0<<CS40); //256
if (mNo == 4) TCCR5B = (0<<ICNC5) | (0<<ICES5) | (1<<WGM53) | (1<<WGM52) | (1<<CS52) | (0<<CS51) | (0<<CS50); //256
prescalerUsed = 256;
}
if (mDelay[mNo-1] / 64 < 65535) {
if (mNo == 1) TCCR1B = (0<<ICNC1) | (0<<ICES1) | (1<<WGM13) | (1<<WGM12) | (0<<CS12) | (1<<CS11) | (1<<CS10); //64
if (mNo == 2) TCCR3B = (0<<ICNC3) | (0<<ICES3) | (1<<WGM33) | (1<<WGM32) | (0<<CS32) | (1<<CS31) | (1<<CS30); //64
if (mNo == 3) TCCR4B = (0<<ICNC4) | (0<<ICES4) | (1<<WGM43) | (1<<WGM42) | (0<<CS42) | (1<<CS41) | (1<<CS40); //64
if (mNo == 4) TCCR5B = (0<<ICNC5) | (0<<ICES5) | (1<<WGM53) | (1<<WGM52) | (0<<CS52) | (1<<CS51) | (1<<CS50); //64
prescalerUsed = 64;
}
if (mDelay[mNo-1] / 8 < 65535) {
if (mNo == 1) TCCR1B = (0<<ICNC1) | (0<<ICES1) | (1<<WGM13) | (1<<WGM12) | (0<<CS12) | (1<<CS11) | (1<<CS10); //8
if (mNo == 2) TCCR3B = (0<<ICNC3) | (0<<ICES3) | (1<<WGM33) | (1<<WGM32) | (0<<CS32) | (1<<CS31) | (1<<CS30); //8
if (mNo == 3) TCCR4B = (0<<ICNC4) | (0<<ICES4) | (1<<WGM43) | (1<<WGM42) | (0<<CS42) | (1<<CS41) | (1<<CS40); //8
if (mNo == 4) TCCR5B = (0<<ICNC5) | (0<<ICES5) | (1<<WGM53) | (1<<WGM52) | (0<<CS52) | (1<<CS51) | (1<<CS50); //8
prescalerUsed = 8;
}
if (mDelay[mNo-1] < 65535) {
if (mNo == 1) TCCR1B = (0<<ICNC1) | (0<<ICES1) | (1<<WGM13) | (1<<WGM12) | (0<<CS12) | (0<<CS11) | (1<<CS10); // none
if (mNo == 2) TCCR3B = (0<<ICNC3) | (0<<ICES3) | (1<<WGM33) | (1<<WGM32) | (0<<CS32) | (0<<CS31) | (1<<CS30); // none
if (mNo == 3) TCCR4B = (0<<ICNC4) | (0<<ICES4) | (1<<WGM43) | (1<<WGM42) | (0<<CS42) | (0<<CS41) | (1<<CS40); // none
if (mNo == 4) TCCR5B = (0<<ICNC5) | (0<<ICES5) | (1<<WGM53) | (1<<WGM52) | (0<<CS52) | (0<<CS51) | (1<<CS50); // none
prescalerUsed = 1;
}
// Set the PWM frequency if not at zero, otherwise stop the timer
if (mDelay[mNo-1] > 0) {
SetPWM(100000000 / mDelay[mNo-1], 50 , prescalerUsed, mNo);
} else {
if (mNo == 1) TCCR1B = (0<<ICNC1) | (0<<ICES1) | (1<<WGM13) | (1<<WGM12) | (0<<CS12) | (0<<CS11) | (0<<CS10); // stopped
if (mNo == 2) TCCR3B = (0<<ICNC3) | (0<<ICES3) | (1<<WGM33) | (1<<WGM32) | (0<<CS32) | (0<<CS31) | (0<<CS30); // stopped
if (mNo == 3) TCCR4B = (0<<ICNC4) | (0<<ICES4) | (1<<WGM43) | (1<<WGM42) | (0<<CS42) | (0<<CS41) | (0<<CS40); // stopped
if (mNo == 4) TCCR5B = (0<<ICNC5) | (0<<ICES5) | (1<<WGM53) | (1<<WGM52) | (0<<CS52) | (0<<CS51) | (0<<CS50); // stopped
}
} //end for,loop
TIMSK1 |= (1 << OCIE1A); //ENABLE interrupt on compare match
TIMSK3 |= (1 << OCIE3A); //ENABLE interrupt on compare match
TIMSK4 |= (1 << OCIE4A); //ENABLE interrupt on compare match
TIMSK5 |= (1 << OCIE5A); //ENABLE interrupt on compare match
}