IMU + Servo

The IMU Gyro now controls the Servo motion. The aim is it compensates for vibration and drift in flight so the attached camera always points stably at the target.  Ultimately, the RPi0W will be attached to the servo with servo movement reversed to achieve this.

The problem at the moment is I can’t find a video player app on a seconds RPI to display the stability of the video output during piDrone instability flight.

2D Servo

First part of the new forward-facing camera for the piDrones is a two-dimensional servo pair for left/right and up/down movement, in it’s basic test as a circle sweep:

from __future__ import division

import math
import time

from RPIO import PWM

RPIO_DMA_CHANNEL = 1

#-------------------------------------------------------------------------------------------
# Set up the globally shared single PWM channel
#-------------------------------------------------------------------------------------------
PWM.set_loglevel(PWM.LOG_LEVEL_ERRORS)
PWM.setup(1)                                    # 1us resolution pulses
PWM.init_channel(RPIO_DMA_CHANNEL, 20000)       # pulse every 20ms


####################################################################################################
#
#  Class for managing each serve via PWM.  Range is 1-2ms every 20ms though specified in micro seconds
#
####################################################################################################
class SERVO:

    def __init__(self, pin):
        #-------------------------------------------------------------------------------------------
        # The GPIO BCM numbered pin providing PWM signal for this ESC
        #-------------------------------------------------------------------------------------------
        self.bcm_pin = pin

        #-------------------------------------------------------------------------------------------
        # Initialize the RPIO DMA PWM for this ESC.
        #-------------------------------------------------------------------------------------------
        self.set(1500)

    def set(self, pulse_width):
        pulse_width = pulse_width if pulse_width >= 1000 else 1000
        pulse_width = pulse_width if pulse_width <= 2000 else 1999

        self.pulse_width = pulse_width

        PWM.add_channel_pulse(RPIO_DMA_CHANNEL, self.bcm_pin, 0, pulse_width)


lr = SERVO(18)
ud = SERVO(23)

try:
    lr.set(1500)
    ud.set(1500)

    while True:
        # Left / Right = 180o Up / Down = 150o
        # Servo is 1000us to 2000us ever 20ms, with 1500us in the middle
        # Ultimately accurate time comes from the IMU
        for ii in range(-100, 101):
            lr.set(1500 + int(round(500 * math.sin(ii / 100 * math.pi))))
            ud.set(1500 + int(round(410 * math.cos(ii / 100 * math.pi))))
            time.sleep(0.02)

except:
    lr.set(1500)
    ud.set(1500)

    del lr
    del ud

    PWM.cleanup()

The only problem to fix was the lower servo that needed dismantling and rebuilding so the centre point of the PWM signal matched the servo physical centre point, not 45 degrees or so out. Check out the difference between my and the supplier’s videos above.