Servos don't work

[Nivek92]

Hey, I don't know why but I can't get my servos to work with this library. Leds are working fine but my servos won't move. I tried everything I can imagine to make it work but I can't and to verify my components are fine I used an esp8266 + arduino with which everything works.

Here is the last iteration of code I tried which is as similar as possible to the arduino code that works.

#![deny(unsafe_code)]
#![no_std]
#![no_main]

use cortex_m_rt::entry;
use panic_semihosting as _;
use pwm_pca9685::{Channel, Pca9685, SlaveAddr};
use stm32f1xx_hal::{
    delay::Delay,
    i2c::{BlockingI2c, DutyCycle, Mode},
    pac,
    prelude::*,
};

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

    let mut flash = dp.FLASH.constrain();
    let mut rcc = dp.RCC.constrain();

    let clocks = rcc.cfgr.freeze(&mut flash.acr);
    let mut afio = dp.AFIO.constrain(&mut rcc.apb2);
    let mut gpiob = dp.GPIOB.split(&mut rcc.apb2);

    let scl = gpiob.pb8.into_alternate_open_drain(&mut gpiob.crh);
    let sda = gpiob.pb9.into_alternate_open_drain(&mut gpiob.crh);
    let mut delay = Delay::new(cp.SYST, clocks);

    let i2c = BlockingI2c::i2c1(
        dp.I2C1,
        (scl, sda),
        &mut afio.mapr,
        Mode::Fast {
            frequency: 400_000.hz(),
            duty_cycle: DutyCycle::Ratio2to1,
        },
        clocks,
        &mut rcc.apb1,
        1000,
        10,
        1000,
        1000,
    );

    let mut pwm = Pca9685::new(i2c, SlaveAddr::default());
    pwm.enable().unwrap();
    pwm.set_prescale(100).unwrap();

    loop {
        for i in 0..4095 {
            pwm.set_channel_on_off(Channel::C0, 0, i % 4095).unwrap();
        }

        for a in 0..=180 {
            pwm.set_channel_on_off(Channel::C10, 0, pulse_width(a))
                .unwrap();
            delay.delay_ms(20_u32);
        }

        delay.delay_ms(500_u32);

        for a in 0..=180 {
            pwm.set_channel_on_off(Channel::C10, 0, pulse_width(180 - a))
                .unwrap();
            delay.delay_ms(20_u32);
        }

        delay.delay_ms(500_u32);
    }
}

fn map_to_range(x: u32, in_min: u32, in_max: u32, out_min: u32, out_max: u32) -> u32 {
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

fn pulse_width(angle: u32) -> u16 {
    let pulse_wide: u32 = map_to_range(angle, 0, 180, 600, 2600);
    let quotient: f64 = pulse_wide as f64 / 1000000.0;
    let analog_value = (quotient * 60.0 * 4096.0) as u16 % 4095;
    return analog_value;
}

Of course I also tried the example code but the servos don't work either. (5v are supplied through the terminal block)

[eldruin]

If you can dim LEDs that means that the address is correct and the communication is working so the difficult part is over. Maybe there is a problem with the math functions. I would recommend getting rid of all that and just get one servo to move to a certain position and then work from there. You can see servos working in this video. Here is the program running there. Also just an STM32F103 "blue pill" board and a PCA9685.

[Nivek92]

I tried the example code before but the servos aren't working. I debugged the code and checked the entries in the values array and they look fine.

I also noticed running the example code all PWM pins on the PCA9685 output the same signal. (Although pins 6,7,8,9 and 15 should be low the whole time.)

Any ideas how to troubleshoot?

[eldruin]

You can try checking what is actually transmitted in the I2C bus and how the PWM pulses look like with a logic analyzer.

[Nivek92]

Yo, I received a logic analyzer so I will try to analyze the signal.

In the meantime I also tried it on my Raspberry Pi Zero. As a reference I used some python code which works and then tried some rust code which should be equal.

Here's the working python code

from __future__ import division
import time

import Adafruit_PCA9685

pwm = Adafruit_PCA9685.PCA9685()

servo_min = 150  # Min pulse length out of 4096
servo_max = 600  # Max pulse length out of 4096

pwm.set_pwm_freq(60)

while True:
    pwm.set_pwm(0, 0, servo_min)
    time.sleep(1)
    pwm.set_pwm(0, 0, servo_max)
    time.sleep(1)

and the not working rust code

use embedded_hal::blocking::delay::DelayMs;
use linux_embedded_hal::{Delay, I2cdev};
use pwm_pca9685::{Address, Channel, Pca9685};

fn main() {
    let dev = I2cdev::new("/dev/i2c-1").unwrap();
    let address = Address::default();
    let mut pwm = Pca9685::new(dev, address).unwrap();

    let servo_min = 150;
    let servo_max = 600;

    let mut delay = Delay;

    // This corresponds to a frequency of 60 Hz.
    pwm.set_prescale(100).unwrap();

    loop {
        pwm.set_channel_on_off(Channel::C0, 0, servo_min).unwrap();
        delay.delay_ms(1000u16);
        pwm.set_channel_on_off(Channel::C0, 0, servo_max).unwrap();
        delay.delay_ms(1000u16);
    }
}

Do I miss something?

[Nivek92]

I recorded the output of the above code via a logic analyzer.

The first is the output from the python code, which works, and the second is from the rust code, which does not work.

[eldruin]

Hi @Nivek92, Thanks for the detailed debugging. The values seem correct:

 0 -> LED0_ON_L
 0 -> LED0_ON_H
 58 -> LED0_OFF_L
 02 -> LED0_OFF_H

This means LED0 on at 0 and off at 570.

I just noticed in your last snippet the call to enable is missing. Could you add that and try again?

I also noticed it is also missing in the driver examples in the documentation. I have noted it and will fix that soon.

[Nivek92]

Hey @eldruin, with pwm.enable().unwrap(); it's actually working. Thank you.

I certainly tried it with pwm.enable().unwrap(); before on my STM32 which didn't work, really weird but now that I have a working reference and the logic analyzer I will also give it another try on the STM32 and report back if there is still a problem.

[eldruin]

I'm glad to hear that :)

[Nivek92]

Ok, so I tried on the STM32 and there it does not work.

Here is the code

#![deny(unsafe_code)]
#![no_std]
#![no_main]

use cortex_m_rt::entry;
use panic_semihosting as _;
use pwm_pca9685::{Address, Channel, Pca9685};
use stm32f1xx_hal::{
    delay::Delay,
    i2c::{BlockingI2c, DutyCycle, Mode},
    pac,
    prelude::*,
};

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

    let mut flash = dp.FLASH.constrain();
    let mut rcc = dp.RCC.constrain();

    let clocks = rcc.cfgr.freeze(&mut flash.acr);
    let mut afio = dp.AFIO.constrain(&mut rcc.apb2);
    let mut gpiob = dp.GPIOB.split(&mut rcc.apb2);

    let scl = gpiob.pb8.into_alternate_open_drain(&mut gpiob.crh);
    let sda = gpiob.pb9.into_alternate_open_drain(&mut gpiob.crh);
    let mut delay = Delay::new(cp.SYST, clocks);

    let i2c = BlockingI2c::i2c1(
        dp.I2C1,
        (scl, sda),
        &mut afio.mapr,
        Mode::Fast {
            frequency: 400_000.hz(),
            duty_cycle: DutyCycle::Ratio2to1,
        },
        clocks,
        &mut rcc.apb1,
        1000,
        10,
        1000,
        1000,
    );

    let mut pwm = Pca9685::new(i2c, Address::default()).unwrap();
    pwm.enable().unwrap();
    pwm.set_prescale(100).unwrap();

    let servo_min = 150;
    let servo_max = 600;

    loop {
        pwm.set_channel_on_off(Channel::C0, 0, servo_min).unwrap();
        // delay
        delay.delay_ms(1000u16);
        pwm.set_channel_on_off(Channel::C0, 0, servo_max).unwrap();
        // delay
        delay.delay_ms(1000u16);
    }
}

memory.x

MEMORY
{
  FLASH : ORIGIN = 0x08000000, LENGTH = 64K
  RAM : ORIGIN = 0x20000000, LENGTH = 20K
}

.cargo/config

[target.thumbv7m-none-eabi]
runner = 'gdb-multiarch'
rustflags = [
  "-C", "link-arg=-Tlink.x",
]

[build]
target = "thumbv7m-none-eabi"

As you can see the frequency is much higher and there are more packets being sent so I couldn't get everything on the screenshot.

All data writes are duplicated. Is this because of DutyCycle::Ratio2to1?

[eldruin]

I will check on this later. In the mean time, you can change the communication mode to standard with:

Mode::Standard {
    frequency: 100_000.hz(),
}

[Nivek92]

Actually tried that already. But the output looks similar.

[eldruin]

What kind of board do you have? Is anything else connected to the I2C bus? Have you tried adding two 10kOhm or 4.7kOhm pull-up resistors to the SCL and SDA lines?

[Nivek92]

Ok, so first off I am an idiot.

The above measurement showed such a high frequent signal because I was running the wrong code. I build in release but loaded from the debug directory. After deploying the correct code it looks much better but it's still not working.

As you can see below it looks nearly the same as the working signal on the raspberry pi but there is a double data write of 06 instead of a single.

And to answer your above questions.

The board is a blue pill but instead of the original STM32 chip it has a CH32 chip but I don't think that makes the difference here.

There is nothing connected to I2C besides the PCA9695 and the logic analyzer.

I tried both 10k and 4.7k pull-up resistors but it didn't make a difference.

Oh and here is the currect code

#![deny(unsafe_code)]
#![no_std]
#![no_main]

use cortex_m_rt::entry;
use panic_semihosting as _;
use pwm_pca9685::{Address, Channel, Pca9685};
use stm32f1xx_hal::{
    delay::Delay,
    i2c::{BlockingI2c, DutyCycle, Mode},
    pac,
    prelude::*,
};

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

    let mut flash = dp.FLASH.constrain();
    let mut rcc = dp.RCC.constrain();

    let clocks = rcc.cfgr.freeze(&mut flash.acr);
    let mut afio = dp.AFIO.constrain(&mut rcc.apb2);
    let mut gpiob = dp.GPIOB.split(&mut rcc.apb2);

    let scl = gpiob.pb8.into_alternate_open_drain(&mut gpiob.crh);
    let sda = gpiob.pb9.into_alternate_open_drain(&mut gpiob.crh);
    let mut delay = Delay::new(cp.SYST, clocks);

    let i2c = BlockingI2c::i2c1(
        dp.I2C1,
        (scl, sda),
        &mut afio.mapr,
        Mode::Fast {
            frequency: 400_000.hz(),
            duty_cycle: DutyCycle::Ratio2to1,
        },
        clocks,
        &mut rcc.apb1,
        1000,
        10,
        1000,
        1000,
    );

    let mut pwm = Pca9685::new(i2c, Address::default()).unwrap();
    pwm.enable().unwrap();
    pwm.set_prescale(100).unwrap();

    let servo_min = 150;
    let servo_max = 600;

    loop {
        pwm.set_channel_on_off(Channel::C0, 0, servo_min).unwrap();
        // delay
        delay.delay_ms(1000u16);
        pwm.set_channel_on_off(Channel::C0, 0, servo_max).unwrap();
        // delay
        delay.delay_ms(1000u16);
    }
}

Also tried the regulat i2c more with 100khz. No difference.

Edit: accidentally closed the issue instead of submitting the comment (was a long day :D)

[eldruin]

Hi @Nivek92, I just built this with a blue-pill, a PCA9685 and one servo similar like in the picture here and your code works fine. Both in fast and standard mode the servo moves from one side to the other as expected, no extra pull-ups needed for me. In fast mode pulseview could not decode the protocol well but in standard it does. You can see there is no extra 06:

This worked fine both in release and debug mode. What I did is just this:

1. openocd -f interface/stlink-v2.cfg -f target/stm32f1x.cfg
2. git clone https://github.com/eldruin/driver-examples
3. cd driver-examples/stm32f1-bluepill
4. nano examples/pca9685.rs
5. Paste your code and save
6. cargo run --example pca9685

Can you try it this way to ensure your problem is not caused by some difference in one of the crates used?

[Nivek92]

Ok, I cloned the above repo and inserted my code, the results are rather interesting. Unfortunately the servo does not move.

When I run it in fast mode I get the correct data writes but still with this weird duplication.

When I run it in standard mode I get the duplicated 06 data write but not the weird duplication.

[eldruin]

I do not know what the problem on your side might be. I cannot reproduce it so I am afraid I cannot provide any further help.

[Nivek92]

Ok no worries.

I also tried it with my STM32F401CEU6, on which it works as well. So it's just the blue pill with the fake chip which does not work.

As it's working with the raspberry pi and the STM32F401 I will move on for now and maybe try to solve the mystery at some other point in time.

Thanks for your time and help. Appreciate it.