New class and code for part 4

src/motor_driver.cpp

@@ -16,7 +16,27 @@ void MotorDriver::begin()
     DDRD |= (1 << DDD3);
     DDRB |= (1 << DDB3);
 
-    stop();
+    // 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();
 }
 
 
@@ -25,8 +45,11 @@ void MotorDriver::forward()
     // AIN1 / D3  = HIGH
     // AIN2 / D11 = LOW
 
-    PORTD |= (1 << PORTD3);
-    PORTB &= ~(1 << PORTB3);
+    //PORTD |= (1 << PORTD3);
+    //PORTB &= ~(1 << PORTB3);
+
+    OCR2A = 0;
+    OCR2B = 255;
 }
 
 
@@ -35,8 +58,11 @@ void MotorDriver::reverse()
     // AIN1 / D3  = LOW
     // AIN2 / D11 = HIGH
 
-    PORTD &= ~(1 << PORTD3);
-    PORTB |= (1 << PORTB3);
+    //PORTD &= ~(1 << PORTD3);
+    //PORTB |= (1 << PORTB3);
+
+    OCR2B = 0;
+    OCR2A = 255;
 }
 
 
@@ -44,6 +70,41 @@ void MotorDriver::stop()
 {
     // Both LOW
 
-    PORTD &= ~(1 << PORTD3);
-    PORTB &= ~(1 << PORTB3);
+    //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();
+    }
 }