-
Notifications
You must be signed in to change notification settings - Fork 213
/
Motors_PWM_Timer.h
170 lines (152 loc) · 7.39 KB
/
Motors_PWM_Timer.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
/*
AeroQuad v3.0.1 - February 2012
www.AeroQuad.com
Copyright (c) 2012 Ted Carancho. All rights reserved.
An Open Source Arduino based multicopter.
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef _AEROQUAD_MOTORS_PWM_TIMER_H_
#define _AEROQUAD_MOTORS_PWM_TIMER_H_
///***********************************************************/
///********************* PWMtimer Motors *********************/
///***********************************************************/
//// Special thanks to CupOfTea for authorting this class
//// http://aeroquad.com/showthread.php?1553-Timed-Motors_PWM
//// Uses system timers directly instead of analogWrite
///*Some basics about the 16 bit timer:
//- The timer counts clock ticks derived from the CPU clock. Using 16MHz CPU clock
// and a prescaler of 8 gives a timer clock of 2MHz, one tick every 0.5us. This
// is also called timer resolution.
//- The timer is used as cyclic upwards counter, the counter period is set in the
// ICRx register. IIRC period-1 has to be set in the ICRx register.
//- When the counter reaches 0, the outputs are set
//- When the counter reaches OCRxy, the corresponding output is cleared.
//In the code below, the period shall be 3.3ms (300hz), so the ICRx register is
// set to 6600 ticks of 0.5us/tick. It probably should be 6599, but who cares about
// this 0.5us for the period. This value is #define TOP
//The high time shall be 1000us, so the OCRxy register is set to 2000. In the code
// below this can be seen in the line "commandAllMotors(1000);" A change of
// the timer period does not change this setting, as the clock rate is still one
// tick every 0.5us. If the prescaler was changed, the OCRxy register value would
// be different.
//*/
///* Motor Mega Pin Port Uno Pin Port HEXA Mega Pin Port
// FRONT 2 PE4 3 PD3
// REAR 3 PE5 9 PB1
// RIGHT 5 PE3 10 PB2 7 PH4
// LEFT 6 PH3 11 PB3 8 PH5
//*/
#include "Arduino.h"
#include "Motors.h"
#if defined (USE_400HZ_ESC)
#define PWM_FREQUENCY 400 // in Hz
#else
#define PWM_FREQUENCY 300 // in Hz
#endif
#define PWM_PRESCALER 8
#define PWM_COUNTER_PERIOD (F_CPU/PWM_PRESCALER/PWM_FREQUENCY)
void initializeMotors(NB_Motors numbers) {
numberOfMotors = numbers;
#if defined (__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
DDRE = DDRE | B00111000; // Set ports to output PE3-5, OC3A, OC3B, OC3C
if (numberOfMotors == FOUR_Motors) {
DDRH = DDRH | B00001000; // Set port to output PH3, OC4A
}
else if (numberOfMotors == SIX_Motors) { // for 6
DDRH = DDRH | B00111000; // Set ports to output PH3-5, OC4A, OC4B, OC4C
}
else if (numberOfMotors == EIGHT_Motors) { // for 8 motor
DDRH = DDRH | B00111000; // Set ports to output PH3-5, OC4A, OC4B, OC4C
DDRB = DDRB | B01100000; // OC1A, OC1B
}
#else
DDRB = DDRB | B00001110; // Set ports to output PB1-3
DDRD = DDRD | B00001000; // Set port to output PD3
#endif
commandAllMotors(1000); // Initialise motors to 1000us (stopped)
#if defined (__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
// Init PWM Timer 3 // WGMn1 WGMn2 WGMn3 = Mode 14 Fast PWM, TOP = ICRn ,Update of OCRnx at BOTTOM
TCCR3A = (1<<WGM31)|(1<<COM3A1)|(1<<COM3B1)|(1<<COM3C1); // Clear OCnA/OCnB/OCnC on compare match, set OCnA/OCnB/OCnC at BOTTOM (non-inverting mode)
TCCR3B = (1<<WGM33)|(1<<WGM32)|(1<<CS31); // Prescaler set to 8, that gives us a resolution of 0.5us
ICR3 = PWM_COUNTER_PERIOD; // Clock_speed / ( Prescaler * desired_PWM_Frequency) #defined above.
if (numberOfMotors == FOUR_Motors) {
// Init PWM Timer 4
TCCR4A = (1<<WGM41)|(1<<COM4A1);
TCCR4B = (1<<WGM43)|(1<<WGM42)|(1<<CS41);
ICR4 = PWM_COUNTER_PERIOD;
}
else if ((numberOfMotors == SIX_Motors) || (numberOfMotors == EIGHT_Motors)) { // for 8 motors
// Init PWM Timer 4
TCCR4A = (1<<WGM41)|(1<<COM4A1)|(1<<COM4B1)|(1<<COM4C1);
TCCR4B = (1<<WGM43)|(1<<WGM42)|(1<<CS41);
ICR4 = PWM_COUNTER_PERIOD;
}
if (numberOfMotors == EIGHT_Motors){ // for 8 motors
// Init PWM Timer 1
TCCR1A = (1<<WGM11)|(1<<COM1A1)|(1<<COM1B1);
TCCR1B = (1<<WGM13)|(1<<WGM12)|(1<<CS11);
ICR1 = PWM_COUNTER_PERIOD;
}
#else
// Init PWM Timer 1 16 bit
TCCR1A = (1<<WGM11)|(1<<COM1A1)|(1<<COM1B1);
TCCR1B = (1<<WGM13)|(1<<WGM12)|(1<<CS11);
ICR1 = PWM_COUNTER_PERIOD;
// Init PWM Timer 2 8bit // WGMn1 WGMn2 = Mode ? Fast PWM, TOP = 0xFF ,Update of OCRnx at BOTTOM
TCCR2A = (1<<WGM20)|(1<<WGM21)|(1<<COM2A1)|(1<<COM2B1); // Clear OCnA/OCnB on compare match, set OCnA/OCnB at BOTTOM (non-inverting mode)
TCCR2B = (1<<CS22)|(1<<CS21); // Prescaler set to 256, that gives us a resolution of 16us
// TOP is fixed at 255 // Output_PWM_Frequency = 244hz = 16000000/(256*(1+255)) = Clock_Speed / (Prescaler * (1 + TOP))
#endif
}
void writeMotors() {
#if defined (__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
OCR3B = motorCommand[MOTOR1] * 2 ;
OCR3C = motorCommand[MOTOR2] * 2 ;
OCR3A = motorCommand[MOTOR3] * 2 ;
OCR4A = motorCommand[MOTOR4] * 2 ;
if (numberOfMotors == SIX_Motors || numberOfMotors == EIGHT_Motors) {
OCR4B = motorCommand[MOTOR5] * 2 ;
OCR4C = motorCommand[MOTOR6] * 2 ;
}
if (numberOfMotors == EIGHT_Motors) {
OCR1A = motorCommand[MOTOR7] * 2 ;
OCR1B = motorCommand[MOTOR8] * 2 ;
}
#else
OCR2B = motorCommand[MOTOR1] / 16 ; // 1000-2000 to 128-256
OCR1A = motorCommand[MOTOR2] * 2 ;
OCR1B = motorCommand[MOTOR3] * 2 ;
OCR2A = motorCommand[MOTOR4] / 16 ;
#endif
}
void commandAllMotors(int command) {
#if defined (__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
OCR3B = command * 2 ;
OCR3C = command * 2 ;
OCR3A = command * 2 ;
OCR4A = command * 2 ;
if (numberOfMotors == SIX_Motors || numberOfMotors == EIGHT_Motors) {
OCR4B = command * 2 ;
OCR4C = command * 2 ;
}
if (numberOfMotors == EIGHT_Motors) {
OCR1A = command * 2 ;
OCR1B = command * 2 ;
}
#else
OCR2B = command / 16 ;
OCR1A = command * 2 ;
OCR1B = command * 2 ;
OCR2A = command / 16 ;
#endif
}
#endif