1 /*
2 OrangutanMotors.cpp - Library for using the motor drivers on the
3 Orangutan LV, SV, SVP, Baby Orangutan B, or 3pi robot.
4 The timer2 overflow ISR is designed to work with Arduino 12
5 and will not work properly with earlier versions of the Arduino
6 environment.
7 */
8
9 /*
10 * Written by Ben Schmidel, May 15, 2008.
11 * Last modified: September 29, 2008
12 * Copyright (c) 2008-2012 Pololu Corporation. For more information, see
13 *
14 * http://www.pololu.com
15 * http://forum.pololu.com
16 * http://www.pololu.com/docs/0J18
17 *
18 * You may freely modify and share this code, as long as you keep this
19 * notice intact (including the two links above). Licensed under the
20 * Creative Commons BY-SA 3.0 license:
21 *
22 * http://creativecommons.org/licenses/by-sa/3.0/
23 *
24 * Disclaimer: To the extent permitted by law, Pololu provides this work
25 * without any warranty. It might be defective, in which case you agree
26 * to be responsible for all resulting costs and damages.
27 */
28
29 #ifndef F_CPU
30 #define F_CPU 20000000UL // Orangutans run at 20 MHz
31 #endif //!F_CPU
32
33 #include <avr/interrupt.h>
34 #include <avr/io.h>
35 #include "OrangutanMotors.h"
36 #include "../OrangutanDigital/OrangutanDigital.h"
37 #include "../OrangutanResources/include/OrangutanModel.h"
38 #ifdef _ORANGUTAN_X2
39 #include "../OrangutanX2/OrangutanX2.h"
40 #endif
41
42 #ifdef _ORANGUTAN_SVP
43
44 #define PWM2A IO_D7
45 #define PWM2B IO_D6
46 #define DIRA IO_C7
47 #define DIRB IO_C6
48
49 #else
50
51 #define PWM0A IO_D6
52 #define PWM0B IO_D5
53 #define PWM2A IO_B3
54 #define PWM2B IO_D3
55
56 #endif
57
58
set_m1_speed(int speed)59 extern "C" void set_m1_speed(int speed)
60 {
61 OrangutanMotors::setM1Speed(speed);
62 }
63
set_m2_speed(int speed)64 extern "C" void set_m2_speed(int speed)
65 {
66 OrangutanMotors::setM2Speed(speed);
67 }
68
set_motors(int m1,int m2)69 extern "C" void set_motors(int m1, int m2)
70 {
71 OrangutanMotors::setSpeeds(m1, m2);
72 }
73
74
75 // constructor
76
OrangutanMotors()77 OrangutanMotors::OrangutanMotors()
78 {
79 }
80
81
82 // initialize timers 0 and 2 to generate the proper PWM ouputs
83 // to the motor drivers
init2()84 void OrangutanMotors::init2()
85 {
86 #ifdef _ORANGUTAN_SVP
87
88 // Configure for non-inverted fast PWM output on motor PWM pins:
89 // Normal port operation, OC2x disconnected (changes later when a non-zero speed is set)
90 // Timer2 counts up from 0 to 255 and then overflows directly to 0.
91 TCCR2A = 0x03;
92
93 // use the system clock/8 (=2.5 MHz) as the timer clock,
94 // which will produce a PWM frequency of 10 kHz
95 TCCR2B = 0x02;
96
97 // use the system clock (=20 MHz) as the timer clock,
98 // which will produce a PWM frequency of 78 kHz. The Baby Orangutan B,
99 // Orangutan SVP, and 3Pi can support PWM frequencies this high. The
100 // Orangutan LV-168 cannot support frequencies above 10 kHz.
101 //TCCR2B = 0x01;
102
103 // Initialize both PWMs to lowest duty cycle possible (almost braking).
104 OCR2A = OCR2B = 0;
105
106 OrangutanDigital::setOutput(DIRA, 0);
107 OrangutanDigital::setOutput(DIRB, 0);
108
109 // Set the PWM pins to be low outputs. They have to be low, otherwise
110 // speed-zero (when OC2x is disconnected) would not work.
111 OrangutanDigital::setOutput(PWM2A, 0);
112 OrangutanDigital::setOutput(PWM2B, 0);
113
114 #elif !defined(_ORANGUTAN_X2)
115
116 /* 40 kHz operation (3pi, Orangutan SV and SVP, and Baby Orangutan B can handle this, Orangutan LV cannot)
117 *** Note that using these settings will break OrangutanTime and QTRsensors ***
118 // configure for inverted phase-correct PWM output on motor control pins:
119 // set OCxx on compare match, clear on timer overflow
120 // Timer0 and Timer2 count up from 0 to 255 and then counts back down to 0
121 TCCR0A = TCCR2A = 0xF1;
122
123 // use the system clock (=20 MHz) as the timer clock,
124 // which will produce a PWM frequency of 39 kHz (because of phase-correct mode)
125 TCCR0B = TCCR2B = 0x01;
126 */
127
128 // configure for inverted fast PWM output on motor control pins:
129 // set OCxx on compare match, clear on timer overflow
130 // Timer0 and Timer2 counts up from 0 to 255 and then overflows directly to 0
131 TCCR0A = TCCR2A = 0xF3;
132
133 #ifndef ARDUINO
134 // use the system clock/8 (=2.5 MHz) as the timer clock,
135 // which will produce a PWM frequency of 10 kHz
136 // Arduino uses Timer0 for timing functions like micros() and delay() so we can't change it
137 TCCR0B = TCCR2B = 0x02;
138 #endif
139
140 // use the system clock (=20 MHz) as the timer clock,
141 // which will produce a PWM frequency of 78 kHz. The Baby Orangutan B
142 // and 3Pi can support PWM frequencies this high. The
143 // Orangutan LV-168 cannot support frequencies above 10 kHz.
144 //TCCR0B = TCCR2B = 0x01;
145
146 // initialize all PWMs to 0% duty cycle (braking)
147 OCR0A = OCR0B = OCR2A = OCR2B = 0;
148
149 OrangutanDigital::setOutput(PWM0A, 0);
150 OrangutanDigital::setOutput(PWM0B, 0);
151 OrangutanDigital::setOutput(PWM2A, 0);
152 OrangutanDigital::setOutput(PWM2B, 0);
153
154 #endif
155 }
156
157
158 // sets the motor speed. The sign of 'speed' determines the direction
159 // and the magnitude determines the speed. limits: -255 <= speed < 255
160 // |speed| = 255 produces the maximum speed while speed = 0 is full brake.
setM1Speed(int speed)161 void OrangutanMotors::setM1Speed(int speed)
162 {
163 #ifdef _ORANGUTAN_X2
164
165 OrangutanX2::setMotor(MOTOR1, IMMEDIATE_DRIVE, speed);
166
167 #else
168
169 init();
170 unsigned char reverse = 0;
171
172 if (speed < 0)
173 {
174 speed = -speed; // make speed a positive quantity
175 reverse = 1; // preserve the direction
176 }
177 if (speed > 0xFF) // 0xFF = 255
178 speed = 0xFF;
179
180 #ifdef _ORANGUTAN_SVP
181
182 OCR2A = speed;
183
184 if (speed == 0)
185 {
186 // Achieve a 0% duty cycle on the PWM pin by driving it low,
187 // disconnecting it from Timer2
188 TCCR2A &= ~(1<<COM2A1);
189 }
190 else
191 {
192 // Achieve a variable duty cycle on the PWM pin using Timer2.
193 TCCR2A |= 1<<COM2A1;
194
195 if (reverse)
196 {
197 OrangutanDigital::setOutput(DIRA, HIGH);
198 }
199 else
200 {
201 OrangutanDigital::setOutput(DIRA, LOW);
202 }
203 }
204
205 #else
206 if (reverse)
207 {
208 OCR0B = 0; // hold one driver input high
209 OCR0A = speed; // pwm the other input
210 }
211 else // forward
212 {
213 OCR0B = speed; // pwm one driver input
214 OCR0A = 0; // hold the other driver input high
215 }
216 #endif // _ORANGUTAN_SVP
217 #endif // _ORANGUTAN_X2
218 }
219
setM2Speed(int speed)220 void OrangutanMotors::setM2Speed(int speed)
221 {
222 #ifdef _ORANGUTAN_X2
223
224 OrangutanX2::setMotor(MOTOR2, IMMEDIATE_DRIVE, speed);
225
226 #else
227
228 init();
229 unsigned char reverse = 0;
230
231 if (speed < 0)
232 {
233 speed = -speed; // make speed a positive quantity
234 reverse = 1; // preserve the direction
235 }
236 if (speed > 0xFF) // 0xFF = 255
237 speed = 0xFF;
238
239 #ifdef _ORANGUTAN_SVP
240
241 OCR2B = speed;
242
243 if (speed == 0)
244 {
245 // Achieve a 0% duty cycle on the PWM pin by driving it low,
246 // disconnecting it from Timer2
247 TCCR2A &= ~(1<<COM2B1);
248 }
249 else
250 {
251 // Achieve a variable duty cycle on the PWM pin using Timer2.
252 TCCR2A |= 1<<COM2B1;
253
254 if (reverse)
255 {
256 OrangutanDigital::setOutput(DIRB, HIGH);
257 }
258 else
259 {
260 OrangutanDigital::setOutput(DIRB, LOW);
261 }
262 }
263
264 #else
265
266 if (reverse)
267 {
268 OCR2B = 0; // hold one driver input high
269 OCR2A = speed; // pwm the other input
270 }
271 else // forward
272 {
273 OCR2B = speed; // pwm one driver input
274 OCR2A = 0; // hold the other driver input high
275 }
276
277 #endif // _ORANGUTAN_SVP
278 #endif // _ORANGUTAN_X2
279 }
280
281
setSpeeds(int m1Speed,int m2Speed)282 void OrangutanMotors::setSpeeds(int m1Speed, int m2Speed)
283 {
284 setM1Speed(m1Speed);
285 setM2Speed(m2Speed);
286 }
287
288 // Local Variables: **
289 // mode: C++ **
290 // c-basic-offset: 4 **
291 // tab-width: 4 **
292 // indent-tabs-mode: t **
293 // end: **
294