Controlling a single servo using PWM and Timer 1

Projects Programming
6

What does work

void setup() {
  
  uint8_t _servoPort = 9; // using pin D9
  uint8_t _servoPinBitmask = digitalPinToBitMask(_servoPort);                             // translate pin number to port register bitmask for selected pin.
  volatile uint8_t *_servoOutputRegister;
  volatile uint8_t *_servoModeRegister;
  _servoOutputRegister = portOutputRegister(digitalPinToPort(_servoPort));        // Get the output (PORT) port register for selected pin.
  _servoModeRegister = (uint8_t *)portModeRegister(digitalPinToPort(_servoPort)); // get DDR register for pin
  *_servoModeRegister |= _servoPinBitmask;                                       // Set pin to output (HIGH) in DDR registe

  // Clear OC1A on match, set on bottom
  TCCR1A |=  (1 << COM1A1);
  TCCR1A &= ~(1 << COM1A0);

  // Set OC1B on match, clear on bottom (inverted)
  TCCR1A |=  (1 << COM1B1);
  TCCR1A |=  (1 << COM1B0);

  // Fast PWM Mode 14 ICR1=TOP
  TCCR1B |=  (1 << WGM13);
  TCCR1B |=  (1 << WGM12);
  TCCR1A |=  (1 << WGM11);
  TCCR1A &= ~(1 << WGM10);

  // frequency 16000000/(8*(40000-1)) = 50Hz
  ICR1  = 0x9C40;

  // Duty cycle 25%
  OCR1A = 0x2710 ;

  // Duty cycle 50%
  OCR1B = 0x4E20 ;


  // Prescaler to CLKio/8
  TCCR1B &= ~(1 << CS12);
  TCCR1B |=  (1 << CS11);
  TCCR1B &= ~(1 << CS10);

}

void loop() {

}

What does not work

#include <LandjeRobot.h>

int servoPin = 9 ;
LandjeRobot landjerobot(servoPin) ;

void setup() {
}

void loop() {
}

LandjeRobot.h

#ifndef LandjeRobot_h
#define LandjeRobot_h

class LandjeRobot
{
public:
   LandjeRobot(int);

private:
  headServoPin ;
};

#endif

LandjeRobot.cpp

#include "LandjeRobot.h"

#include "LandjeServo.h"

LandjeServo z;

LandjeRobot::LandjeRobot(int headServo)
{
    _headServoPin = headServo ;

    z.Init(_headServoPin) ;

}

LandjeServo.h

#ifndef LandjeServo_h
#define LandjeServo_h

#include <Arduino.h>

class LandjeServo
{

public:
  Init(int servo);

private:
  uint8_t _servoPort;
  uint8_t _servoPinBitmask;
  volatile uint8_t *_servoOutputRegister;
  volatile uint8_t *_servoModeRegister;
  unsigned int _min;
  unsigned int _max;
};

#endif

LandjeServo.cpp

#include <Arduino.h>
#include "LandjeServo.h"

LandjeServo::Init(int servoPin)
{

    _servoPort = servoPin;                                                          // using pin D9
    _servoPinBitmask = digitalPinToBitMask(_servoPort);                             // translate pin number to port register bitmask for selected pin.
    _servoOutputRegister = portOutputRegister(digitalPinToPort(_servoPort));        // Get the output (PORT) port register for selected pin.
    _servoModeRegister = (uint8_t *)portModeRegister(digitalPinToPort(_servoPort)); // get DDR register for pin
    *_servoModeRegister |= _servoPinBitmask;                                        // Set pin to output (HIGH) in DDR registe

   // Clear OC1A on match, set on bottom
    TCCR1A |= (1 << COM1A1);
    TCCR1A &= ~(1 << COM1A0);

    // Set OC1B on match, clear on bottom (inverted)
    TCCR1A |= (1 << COM1B1);
    TCCR1A |= (1 << COM1B0);

    // Fast PWM Mode 14 ICR1=TOP
    TCCR1B |= (1 << WGM13);
    TCCR1B |= (1 << WGM12);
    TCCR1A |= (1 << WGM11);
    TCCR1A &= ~(1 << WGM10);

    // frequency 16000000/(8*(40000-1)) = 50Hz
    ICR1 = 0x9C40;

    // Duty cycle 25%
    OCR1A = 0x2710;

    // Duty cycle 50%
    OCR1B = 0x4E20;

    // Prescaler to CLKio/8
    TCCR1B &= ~(1 << CS12);
    TCCR1B |= (1 << CS11);
    TCCR1B &= ~(1 << CS10);
};