400Hz PWM on Atmega32u4 for multirotors (without using Servo Library)
!! Code available on my GitHub toskyRocker account !!
The main issue using the Servo Library is that you can hardly go faster than 125Hz, with a huge amount of interrupts generated. I use instead 16bit Timer1 and Timer3 to generate PWM signals @400Hz on 4 pins (up to 6 pins). No interrupts are required.
First of all you need to know what a timers and interrupts are: Timers and Interrups on Arduino
Once you got some basic info you can read the Atmega32u4 Datasheet for a better comprehension of Timer1 and Timer3 adopted in my code working on any Arduino with Atmega32u4 (Arduino Yun, Micro, Leonardo)
/*
Andrea Toscano 2015
Microcontroller : Atmega32u4
PWM FREQUENCY @ 400Hz for multicopters
USE PIN 5,9,10,11*/
#define motor0 OCR1A // PIN 9
#define motor1 OCR1B // PIN 10
#define motor2 OCR1C // PIN 11
#define motor3 OCR3A // PIN 5
/* These values depend on ICR1 and ICR3
You may vary the following parameters if your
ESCs seem
*/
#define MIN 2200
#define MAX 3800
int motorValue = 0;
unsigned long newMills = 0;
unsigned long oldMills = 0;
void setup() {
//*************************************************
// Timer 1
//*************************************************
// Pin Setup
// Output Direction
DDRB |= (1 << 7) | (1 << 6) | (1 << 5);
//
ICR1 = 0x1387; // 400Hz
/*
[COM1A1,COM1A0,COM1B1,COM1B0,COM1C1,COM1C0,WGM11.WGM10]
bit 7:2 Compare output mode 3 channels.
Setting OCnA/OCnB/OCnC on comptare match, set output to LOW
bit 1:0 Fast PWM, TOP: ICRn Modality
*/
TCCR1A = 0b10101010;
/*
[ICNC1,ICES1,–,WGM13,WGM12,CS12,CS11,CS10]
bit 7:5 Don't Care
bit 4:3 Fast PWM
bit 2:0 Prescaler: clock/8
*/
TCCR1B = 0b00011010;
// Initializing output variables
motor0 = 0;
motor1 = 0;
motor2 = 0;
//*************************************************
// Timer 3
//*************************************************
DDRC |= (1 << PC6); // motor3 PWM Port.
ICR3 = 0x1387;
TCCR3A = 0b10101010;
TCCR3B = 0b00011010;
// Initializing output var
motor3 = 0;
motorValue = MIN;
}
/* Test the duty cycle with an oscilloscope
Loop will vary from min to max
*/
void loop()
{
newMills = millis();
if (newMills - oldMills > 100)
{
motorValue += 5;
if (motorValue == MAX)
motorValue = MIN;
motor0 = motorValue;
motor1 = motorValue;
motor2 = motorValue;
motor3 = motorValue;
oldMills = newMills;
Serial.println(motorValue);
}
}
Comments (6)
Christopher
Nov 14, 2016 at 4:53 pmIs it possible to integrate that with the PID Library? If so, how can I set the output of the PID calculation to be according to the PWM generated by this code?
Thanks!
AndrewTosky
Nov 14, 2016 at 10:45 pmHi,
Sure you can integrate it. It’s what I’ve done in my Comelicottero project:
https://github.com/toskyRocker/Comelicottero_Arduino_Yun .
PID doesn’t need to know anything about the nature of the input.
You only need to find the coefficient and provide MIN and MAX boundary values to prevent the integral overshoot.
Chris
Mar 07, 2017 at 7:55 pmSince the OCR register only receives integers, does that mean that your PID works with integers only? Is it precise enough?
Is there any way to work with floats on the PID and set the OCR register to a float number (which would be the PID output)?
Thanks!
AndrewTosky
Mar 07, 2017 at 8:09 pmHi Chris,
First of all, the control system chain looks like this:
IMU signals -> Filters -> PIDs -> Motor Compensation Filter -> PWM Generation -> | -> ESCs -> Motors
PIDs of course work on floats since they receive inertial signals.
My PIDs are also optimised for quadcopter usage.
Have a look here https://github.com/toskyRocker/Comelicottero_Arduino_Yun/blob/master/PID.cpp .
PWM signals generation happens afterwards and it’s based on integers.
From my experience I think the granularity is fine enough, you don’t loose much precision.
Cheers
Andrea
Chris
Mar 09, 2017 at 6:37 pmOK, got it.
Another question: I see you set the ICR resgister to 4999, does that mean that your maximum duty cycle will be from 0-4999? Also, what is the formula to get this ICR value according to frequency?
Lastly, why do you use fast PWM instead of phase correct? I thought phase correct was the best choice for motor control…
Thanks!
AndrewTosky
Mar 10, 2017 at 1:29 amHi Chris,
The duty cycle is defined by ICR / prescaler. Since ICR has 5000 values and the prescaler is 8 (check TCCR1x registers) I get a 400Hz PWM signal as outcome.
ESCs are only interested in the length of the pulse, no matter which technique you adopt 🙂
Cheers
Andrea