Romi32U4 library
Romi32U4Motors.cpp
1 // Copyright Pololu Corporation. For more information, see http://www.pololu.com/
2 
3 #include <Romi32U4Motors.h>
4 #include <FastGPIO.h>
5 #include <avr/io.h>
6 
7 #define PWM_L 10
8 #define PWM_R 9
9 #define DIR_L 16
10 #define DIR_R 15
11 
12 bool Romi32U4Motors::flipLeft = false;
13 bool Romi32U4Motors::flipRight = false;
14 
15 int16_t Romi32U4Motors::maxSpeed = 300;
16 
17 // initialize timer1 to generate the proper PWM outputs to the motor drivers
18 void Romi32U4Motors::init2()
19 {
24 
25  // Timer 1 configuration
26  // prescaler: clockI/O / 1
27  // outputs enabled
28  // phase-correct PWM
29  // top of 400
30  //
31  // PWM frequency calculation
32  // 16MHz / 1 (prescaler) / 2 (phase-correct) / 400 (top) = 20kHz
33  TCCR1A = 0b10100000;
34  TCCR1B = 0b00010001;
35  ICR1 = 400;
36  OCR1A = 0;
37  OCR1B = 0;
38 }
39 
41 {
42  flipLeft = flip;
43 }
44 
46 {
47  flipRight = flip;
48 }
49 
50 void Romi32U4Motors::setLeftSpeed(int16_t speed)
51 {
52  init();
53 
54  bool reverse = 0;
55 
56  if (speed < 0)
57  {
58  speed = -speed; // Make speed a positive quantity.
59  reverse = 1; // Preserve the direction.
60  }
61  if (speed > maxSpeed)
62  {
63  speed = maxSpeed;
64  }
65 
66  OCR1B = speed;
67 
68  FastGPIO::Pin<DIR_L>::setOutput(reverse ^ flipLeft);
69 }
70 
71 void Romi32U4Motors::setRightSpeed(int16_t speed)
72 {
73  init();
74 
75  bool reverse = 0;
76 
77  if (speed < 0)
78  {
79  speed = -speed; // Make speed a positive quantity.
80  reverse = 1; // Preserve the direction.
81  }
82  if (speed > maxSpeed)
83  {
84  speed = maxSpeed;
85  }
86 
87  OCR1A = speed;
88 
89  FastGPIO::Pin<DIR_R>::setOutput(reverse ^ flipRight);
90 }
91 
92 void Romi32U4Motors::setSpeeds(int16_t leftSpeed, int16_t rightSpeed)
93 {
94  setLeftSpeed(leftSpeed);
95  setRightSpeed(rightSpeed);
96 }
97 
99 {
100  maxSpeed = turbo ? 400 : 300;
101 }
static void setOutput(bool value) __attribute__((always_inline))
Sets the pin as an output.
Definition: FastGPIO.h:260
static void flipRightMotor(bool flip)
Flips the direction of the right motor.
static void setRightSpeed(int16_t speed)
Sets the speed for the right motor.
static void flipLeftMotor(bool flip)
Flips the direction of the left motor.
static void allowTurbo(bool turbo)
Turns turbo mode on or off.
static void setSpeeds(int16_t leftSpeed, int16_t rightSpeed)
Sets the speed for both motors.
static void setOutputLow() __attribute__((always_inline))
Configures the pin to be an output driving low.
Definition: FastGPIO.h:226
static void setLeftSpeed(int16_t speed)
Sets the speed for the left motor.