How to control Micro servo SG90 more precisely?

[katethemate]

Hey! ^^ We want to control the servo SG90 with an Arduino UNO using Rust. We made this work, however not as precisely as we would like and were able to achieve with C++ (1 degree precision).

Setting the pin to 0 moves the servo to 0 degrees and setting the pin to 30 moves the servo to 180 degrees. Since the duty is of type u8 we're only able to control the servo in 6 degree steps which isn't precise enough for our use case.

We were wondering whether there is a possibility to control it more precisely? Maybe it is possible to use f64 instead of u8?

#![no_std]
#![no_main]

use arduino_uno::{
    pac::TC2,
    prelude::*,
    pwm::{self, Timer2Pwm},
    Peripherals, Pins, DDR,
};
use atmega328p_hal::port::{
    mode::{Floating, Input, Pwm},
    portd::PD3,
};
use panic_halt as _;

#[arduino_uno::entry]
fn main() -> ! {
    let dp = Peripherals::take().unwrap();
    let mut pins = Pins::new(dp.PORTB, dp.PORTC, dp.PORTD);

    let mut timer = Timer2Pwm::new(dp.TC2, pwm::Prescaler::Prescale256);
    let mut pin = pins.d3.into_output(&mut pins.ddr).into_pwm(&mut timer);
    pin.enable();

    loop {
        pin.set_duty(0); // 0°
        arduino_uno::delay_ms(2000u16);

        pin.set_duty(30); // 180°
        arduino_uno::delay_ms(2000u16);
    }
}

Thank you so much for taking the time to read!

[Rahix]

Hello!

So I was writing up a super long answer to your question, with details why it is the way it is right now and how one could go about fixing it and just as I got towards the end of it I noticed a little detail which makes the whole thing much harder and renders my answer kind of irrelevant :(

To sum it up in a few words: The way your code is generating the PWM signal to control the servo is much different from the way the Arduino Servo library does it. With your method it is unfortunately impossible to archieve higher precision (well, except for one specific method that only works for pin D10) so one would need to re-implement the Arduino Servo code in Rust.

If you are curious about more details why this is and you aren't scared of gory low-level hardware stuff, I can try to recycle at least parts of my original answer :)

Now, as I said, the only realistic way I see for archieving high precision servo control is by going a similar route to what the Servo library for Arduino C++ is doing. I am unfortunately too short on time to build something like this right now, but if you are interested, I could offer to guide you through it.

Sorry for not having better news right now...

[Urhengulas]

Hi @Rahix,

Thank you very much for your answer! (I am working on this together with @katethemate)

I would be very interested in why it doesn't work with the way we approached it, since for both of us it's the first deep dive into embedded development and want to learn as much as possible! (We stole that from Dajamante/avr-car, btw 😁)

We expected to need to port over some C/C++ libraries, or write an rust ffi interface for them. It would be tremendously awesome if you could guide us in doing so!

Thank you a lot for your time and energy!

[Rahix]

Sounds good! I think the easiest way to get started would be to have a quick chat/jitsi call to talk about this, if you two are okay with that. Otherwise I can also try writing all the things down but this would take me some time and in my experience makes everything a bit slower... I can definitely find some spare time tomorrow evening or this weekend if you're interested :)

[Urhengulas]

A call would be awesome! We are at CET and tomorrow after 6pm or weekend should work, but I will check back with @katethemate to make sure.

Can you please send me a mail to johann.hemmann@code.berlin so we can figure out the scheduling there? 📬

[koutoftimer]

@Urhengulas take a look at https://github.com/Rahix/avr-hal/pull/136 I've solved exactly your problem for SG 90 servo. What I have found out is that SG 90 is pretty inaccurate and you need to figure out actual pulse width/duty required for your device empirically.

Currently avr-hal experiencing some internal design changes, I have no exact vision of how it actually should look like so I'm just waiting for it to finish.

[Rahix]

Just for any future readers of this issue, I'll document how one can manually configure a timer to produce the PWM signal needed for this (@Urhengulas, @katethemate, this is just the code we discussed):

With the current state on the master branch of avr-hal, this is some sample code to control a servo with ~1 degree of precision (in the PWM signal, the mechanical precision of the servo is of course a limiting factor):

#![no_std]
#![no_main]

use arduino_uno::{
    pac::TC1,
    prelude::*,
    pwm::{self, Timer2Pwm},
    Peripherals, Pins, DDR,
};
use atmega328p_hal::port::{
    mode::{Floating, Input, Pwm},
    portd::PD3,
};
use panic_halt as _;

#[arduino_uno::entry]
fn main() -> ! {
    let dp = Peripherals::take().unwrap();
    let mut pins = Pins::new(dp.PORTB, dp.PORTC, dp.PORTD);

    // Important because this sets the bit in the DDR register!
    pins.d9.into_output(&mut pins.ddr);

    // - TC1 runs off a 250kHz clock, with 5000 counts per overflow => 50 Hz signal.
    // - Each count increases the duty-cycle by 4us.
    // - Use OC1A which is connected to D9 of the Arduino Uno.
    let tc1 = dp.TC1;
    tc1.icr1.write(|w| unsafe { w.bits(4999) });
    tc1.tccr1a.write(|w| w.wgm1().bits(0b10).com1a().match_clear());
    tc1.tccr1b.write(|w| w.wgm1().bits(0b11).cs1().prescale_64());

    loop {
        // 100 counts => 0.4ms
        // 700 counts => 2.8ms
        for duty in 100..=700 {
            tc1.ocr1a.write(|w| unsafe { w.bits(duty) });
            arduino_uno::delay_ms(20);
        }
    }
}

[joshuajbouw]

@Rahix I'm a bit confused as to where the 0.4ms and 2.8ms counts come from when reading up on PWM signals for a servo. How would I know what is 0 degrees and 180 degrees?

[Rahix]

By the SG90 datasheet, the servo expects a 50Hz PWM signal with a varying duty-cycle between 1ms (left limit) and 2ms (right limit). These values aren't really exact so the values here start a bit lower than 1ms and go a bit higher than 2ms. You could now check what exact value the limits correspond to - but note that this will differ from model to model.

[Rahix]

Btw, here is an updated version of the above example for new avr-hal (#130):

#![no_std]
#![no_main]

use panic_halt as _;

#[arduino_hal::entry]
fn main() -> ! {
    let dp = arduino_hal::Peripherals::take().unwrap();
    let pins = arduino_hal::pins!(dp);

    // Important because this sets the bit in the DDR register!
    pins.d9.into_output();

    // - TC1 runs off a 250kHz clock, with 5000 counts per overflow => 50 Hz signal.
    // - Each count increases the duty-cycle by 4us.
    // - Use OC1A which is connected to D9 of the Arduino Uno.
    let tc1 = dp.TC1;
    tc1.icr1.write(|w| unsafe { w.bits(4999) });
    tc1.tccr1a.write(|w| w.wgm1().bits(0b10).com1a().match_clear());
    tc1.tccr1b.write(|w| w.wgm1().bits(0b11).cs1().prescale_64());

    loop {
        // 100 counts => 0.4ms
        // 700 counts => 2.8ms
        for duty in 100..=700 {
            tc1.ocr1a.write(|w| unsafe { w.bits(duty) });
            arduino_hal::delay_ms(20);
        }
    }
}

[joshuajbouw]

Ah fantastic. Also, had figured out the updated code already so no problem. It was hard to find the MH995 duty cycle, which is why I was getting confused as in the data sheet it actually doesn't say interestingly enough. But, turns out it is 0.5ms -> 2.5ms. Getting 180 degrees of rotation now. Thanks.

[robiot]

Hello @Rahix ! I have been playing around a bit with this library recently and I'm trying to build a kind of wrapper :shrug: around servo integration.

This is what I have come up with but it has some issues:

1. It only works with D9 and D10
2. It does not work with two instances simultaneously. ex D9 and D10.

I guess (2.) is because the tc1.tccr1a gets overwritten to use ex. .com1b instead of .com1a. Is there any other register I can use instead of tccr1a that works the same?

I'm also trying to understand how the Servo library for c++ works, but it's a lot of new words and and stuff since I am pretty new to Arduino development, and embedded development in general :sweat_smile:.

From this part of the Servo code, it looks like it's also using the oscillator. But how does it still work on all the pins that doesn't support oscillation, and with multiple servos at the same time?

static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA)
{
  if( Channel[timer] < 0 )
    *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
  else{
    if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true )
      digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated
  }

  Channel[timer]++;    // increment to the next channel
  if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
    *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
    if(SERVO(timer,Channel[timer]).Pin.isActive == true)     // check if activated
      digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
  }
  else {
    // finished all channels so wait for the refresh period to expire before starting over
    if( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) )  // allow a few ticks to ensure the next OCR1A not missed
      *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
    else
      *OCRnA = *TCNTn + 4;  // at least REFRESH_INTERVAL has elapsed
    Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
  }
}

The initial comment on this issue uses some kind of Timer2Pwm, is it possible to use that with multiple servos and with the precision of the manual oscillation thing (ex. ocr1a)?

my solution which only works for one pin at a time :(:

use arduino_hal::pac::TC1;

pub enum ServoPins {
    // D6,  // OC0A
    // D5,  // 0C0B
    D9, // 0C1A
    D10, // 0C1B
        // D11, // 0C2A
        // D3,  // 0C2B
}

pub struct Servo {
    pin: ServoPins,
    last_to: i32,
    tc1: TC1,
}

impl Servo {
    pub fn write(&mut self, degrees: i32) {
        let mut degrees = if degrees > 180 {
            180
        } else if degrees < 0 {
            0
        } else {
            degrees
        };

        // Magic math stuff
        let servo_up_time_ms = (degrees as f64) * 0.011111 + 0.5;

        // - Each count increases the duty-cycle by 4us = 0.004ms.
        let value_to_write = (servo_up_time_ms / 0.004) as u16;

        // Writes the new time to be HIGH to the representing oscillator.
        match self.pin {
            ServoPins::D9 => 
                self.tc1.ocr1a.write(|w| unsafe { w.bits(value_to_write) }),
            ServoPins::D10 => self.tc1.ocr1b.write(|w| unsafe { w.bits(value_to_write) }),
        }

        self.last_to = degrees;
    }

    /**
     * Creates an servo instance for specified pin
     * Basically like the attach function in Servo.h
     */
    pub fn attach(pin: ServoPins, initial_degrees: i32) -> Servo {
        let dp = unsafe { arduino_hal::Peripherals::steal() };
        let pins = arduino_hal::pins!(dp);

        // Initialize the oscillator
        // - TC1 runs off a 250kHz clock, with 5000 counts per overflow => 50 Hz signal.
        let tc1 = dp.TC1;
        tc1.icr1.write(|w| unsafe { w.bits(4999) });

        tc1.tccr1b
            .write(|w| w.wgm1().bits(0b11).cs1().prescale_64());

        // Set the used pin to output mode, write to tccr1a with the corresponding compare output
        match pin {
            ServoPins::D9 => {
                pins.d9.into_output();
                tc1.tccr1a
                    .write(|w| w.wgm1().bits(0b10).com1a().match_clear());
            }
            ServoPins::D10 => {
                pins.d10.into_output();
                tc1.tccr1a
                    .write(|w| w.wgm1().bits(0b10).com1b().match_clear());
            }
        }

        let mut servo = Servo {
            pin,
            last_to: initial_degrees,
            tc1,
        };

        // Write initial degrees
        servo.write(initial_degrees);

        servo
    }
}

edit: I am using an arduino uno.

[robiot]

I just tried this, and it doesn't move at all. When the duty is set to 255, the servo starts vibrating but still doesn't move.

    let mut timer = Timer1Pwm::new(dp.TC1, Prescaler::Prescale256);
    let mut pin = pins.d9.into_output().into_pwm(&mut timer);

    pin.enable();

    loop {
        pin.set_duty(0); // 0°

        arduino_hal::delay_ms(1000);

        pin.set_duty(255);

        arduino_hal::delay_ms(1000);
   }

[pmnlla]

I apologize for the necro.

I've started documenting my progress on more precise servo control here: https://github.com/Rahix/avr-hal/discussions/489#discussioncomment-8495325

I figured anyone else who's running into issues with servos or knows the atmega328p better than I do might be able to help.