123 lines
1.7 KiB
C++
123 lines
1.7 KiB
C++
#include "motor_driver.h"
|
|
|
|
#include <avr/io.h>
|
|
|
|
|
|
MotorDriver::MotorDriver()
|
|
{
|
|
}
|
|
|
|
|
|
void MotorDriver::begin()
|
|
{
|
|
// Arduino D3 = PD3 = AIN1
|
|
// Arduino D11 = PB3 = AIN2
|
|
// Arduino D4 = PD4 = FLT
|
|
|
|
DDRD |= (1 << DDD3);
|
|
DDRB |= (1 << DDB3);
|
|
|
|
DDRD &= ~(1 << DDD4);
|
|
|
|
PORTD |= (1 << PORTD4); // Enable pull-up resistor on D4 for part 2
|
|
|
|
|
|
// Timer2 Fast PWM
|
|
TCCR2A = 0;
|
|
TCCR2B = 0;
|
|
|
|
// Fast PWM mode
|
|
TCCR2A |= (1 << WGM20);
|
|
TCCR2A |= (1 << WGM21);
|
|
|
|
// Non-inverting PWM on OC2A and OC2B
|
|
TCCR2A |= (1 << COM2A1);
|
|
TCCR2A |= (1 << COM2B1);
|
|
|
|
|
|
// Timer2 prescaler = 32
|
|
TCCR2B |= (1 << CS21);
|
|
TCCR2B |= (1 << CS20);
|
|
|
|
OCR2A = 0;
|
|
OCR2B = 0;
|
|
|
|
//stop();
|
|
}
|
|
|
|
|
|
void MotorDriver::forward()
|
|
{
|
|
// AIN1 / D3 = HIGH
|
|
// AIN2 / D11 = LOW
|
|
|
|
//PORTD |= (1 << PORTD3);
|
|
//PORTB &= ~(1 << PORTB3);
|
|
|
|
OCR2A = 0;
|
|
OCR2B = 255;
|
|
}
|
|
|
|
|
|
void MotorDriver::reverse()
|
|
{
|
|
// AIN1 / D3 = LOW
|
|
// AIN2 / D11 = HIGH
|
|
|
|
//PORTD &= ~(1 << PORTD3);
|
|
//PORTB |= (1 << PORTB3);
|
|
|
|
OCR2B = 0;
|
|
OCR2A = 255;
|
|
}
|
|
|
|
|
|
void MotorDriver::stop()
|
|
{
|
|
// Both LOW
|
|
|
|
//PORTD &= ~(1 << PORTD3);
|
|
//PORTB &= ~(1 << PORTB3);
|
|
|
|
OCR2A = 0;
|
|
OCR2B = 0;
|
|
}
|
|
|
|
void MotorDriver::set(double pwm)
|
|
{
|
|
// Limit PWM command
|
|
if (pwm > 255.0)
|
|
{
|
|
pwm = 255.0;
|
|
}
|
|
|
|
if (pwm < -255.0)
|
|
{
|
|
pwm = -255.0;
|
|
}
|
|
|
|
|
|
if (pwm > 0)
|
|
{
|
|
// Forward
|
|
OCR2A = 0;
|
|
OCR2B = (uint8_t)pwm;
|
|
}
|
|
else if (pwm < 0)
|
|
{
|
|
// Reverse
|
|
OCR2B = 0;
|
|
OCR2A = (uint8_t)(-pwm);
|
|
}
|
|
else
|
|
{
|
|
stop();
|
|
}
|
|
}
|
|
|
|
|
|
bool MotorDriver::fault_active()
|
|
{
|
|
return !(PIND & (1 << PIND4));
|
|
}
|