User avatar
jbonnett
Posts: 29
Joined: Mon Jan 21, 2013 2:13 am

Accurately controlling a Servo

Sun Apr 14, 2013 2:23 am

I know I need to add a while loop somewhere, I have tried adding one in a few places although no joy, now I'm thinking is there a way to change the for loop into a while? The problem is that when I run the program and move the joystick fast the servo's position stops half way compared to the joystick, which tells me that the signal to the servo is sent once and not repeated to correct the servo position.

I never created the "xbox_read.py" I only created the "servos.py".

servos.py

Code: Select all

import RPi.GPIO as GPIO
import time
import xbox_read
from threading import Thread
import smbus
import sys
import getopt

def new_value(current, pos): # Function for changing range
	if pos == "left-up":
        	return (((current - 0) * (180 - 90)) / (100 - 0)) + 90 # Change range from 0 - 100 to 90 - 180
	elif pos == "right-down":
		return (((current - 0) * (0 - 90)) / (100 - 0)) + 90 # Change range from 0 - 100 to 90 - 0

bus = smbus.SMBus(1)

address = 0x20 # I2C address of MCP23017
bus.write_byte_data(address,0x00,0x00) # Set all of bank A to outputs
bus.write_byte_data(address,0x01,0x00) # Set all of bank B to outputs

GPIO.setmode(GPIO.BOARD) # Set pin layout
GPIO.setwarnings(False) # If warning don't show them
GPIO.setup(10, GPIO.OUT) # Set GPIO pin that servo 1 as output
GPIO.setup(12, GPIO.OUT) # Set GPIO pin that servo 2 as output

def move_servo(degrees=90, servo=0): # Function that moves servo position
	pos_servo = ((0.0000122 * degrees) + 0.0002) # Veriable that calculates new servo position
        if servo == 1: # If servo 1
	        GPIO.output(10, True) # Turn servo 1 GPIO on
                time.sleep(pos_servo) # Wait
                GPIO.output(10, False) # Turn servo 1 GPIO off
                time.sleep(0.0025 - pos_servo) # Wait
	elif servo == 2: # If servo 2
                GPIO.output(12, True) # Turn servo 2 GPIO on
                time.sleep(pos_servo) # Wait
                GPIO.output(12, False) # Turn servo 2 GPIO off
                time.sleep(0.0025 - pos_servo) # Wait
	else: # If no servo stated move both to 0
                GPIO.output(10, True) # Turn servo 1 GPIO on
                GPIO.output(12, True) # Turn servo 2 GPIO on
                time.sleep(pos_servo) # Wait
                GPIO.output(10, False) # Turn servo 1 GPIO off
		GPIO.output(12, False) # Turn servo 2 GPIO off

def right_joystic():
	for event in xbox_read.event_stream(deadzone=2000, scale=100): # Loop that looks for changes from Xbox controler
		if event.key=='X2': # If change 'right joystic' moved left or right
			bus.write_byte_data(address,0x13,0x1) # Turn MCP23017 port 1 on for activity light
			x2_intensity = int(event.value) # Set varible for position by intensity 
			if x2_intensity >= 0: # If position of 'right joystic' right 'more than or 0'
	                        move_servo(new_value(x2_intensity, "right-down"), 1) # Change range and move servo to new position
	                elif x2_intensity <= 0: # If position of 'right joystic' left 'less than or 0'
	                        move_servo(new_value(x2_intensity*-1, "left-up"), 1) # Change range and move servo to new position
			else: # If position of 'right joystic' left or right 0
				move_servo(0, 1) # Move servo to new position
			bus.write_byte_data(address,0x13,0x00) # Turn MCP23017 port 1 off for activity light
		elif event.key=='Y2': # If change 'right joystic' moved up or down
	                bus.write_byte_data(address,0x13,0x1) # Turn MCP23017 port 1 on for activity light
	                y2_intensity = int(event.value) # Set varible for position by intensity
	                if y2_intensity >= 0: # If position of 'right joystic' down 'more than or 0'
	                       move_servo(new_value(y2_intensity, "right-down"), 2) # Change range and move servo to new position 
	                elif y2_intensity <= 0: # If position of 'right joystic' up 'less than or 0'
	                        move_servo(new_value(y2_intensity*-1, "left-up"), 2) # Change range and move servo to new position
	                else: # If position of 'right joystic' up or down 0
	                        move_servo(0, 2) # Move servo to new position
	                bus.write_byte_data(address,0x13,0x00) # Turn MCP23017 port 1 off for activity light

Thread(target=move_servo).start() # Set new thread for moving servos
Thread(target=right_joystic).start() # Set new thread for controls


xbox_read.py

Code: Select all

from os import popen
from sys import stdin
import re

s = re.compile('[ :]')

class Event:
    def __init__(self,key,value,old_value):
        self.key = key
        self.value = value
        self.old_value = old_value
    def is_press(self):
        return self.value==1 and self.old_value==0
    def __str__(self):
        return 'Event(%s,%d,%d)' % (self.key,self.value,self.old_value)

def apply_deadzone(x, deadzone, scale):
    if x < 0:
        return (scale * min(0,x+deadzone)) / (32768-deadzone)
    return (scale * max(0,x-deadzone)) / (32767-deadzone)

def event_stream(deadzone=0,scale=32768):
    _data = None
    subprocess = popen('nohup xboxdrv','r',65536)
    while (True):
        line = subprocess.readline()
        if 'Error' in line:
            raise ValueError(line)
        data = filter(bool,s.split(line[:-1]))
        if len(data)==42:
            # Break input string into a data dict
            data = { data[x]:int(data[x+1]) for x in range(0,len(data),2) }
            if not _data:
                _data = data
                continue
            for key in data:
                if key=='X1' or key=='X2' or key=='Y1' or key=='Y2':
                    data[key] = apply_deadzone(data[key],deadzone,scale)
                if data[key]==_data[key]: continue
                event = Event(key,data[key],_data[key])
                yield event
            _data = data

# Appendix: Keys
# --------------
# X1
# Y1
# X2
# Y2
# du
# dd
# dl
# dr
# back
# guide
# start
# TL
# TR
# A
# B
# X
# Y
# LB
# RB
# LT
# RT

BeeryChisels
Posts: 18
Joined: Sat Apr 13, 2013 7:00 pm

Re: Accurately controlling a Servo

Sun Apr 14, 2013 4:34 pm

This is most unusual behaviour for a servo. They should go where they are told, slowly or quickly depending on the servo. However, only going part of the way is something which can happen quite easily with stepper motors.
The servo motors that I know of are controlled by a PWM signal, so I assume that the digital value obtained from the A-to-D on the joystick output is used to load a counter which is then used to provide the 'mark' time. The 'space' time will then be the time remaining out of each complete cycle, and continuously repeating cycles as defined by these two times will be the output from the Pi. Or there may be chips around these days that produce a PWM output directly from an analogue input - in which case the Pi is not really necessary, so there must be more to this problem than meets the eye.
However it is done, it is difficult to imagine what can go wrong to produce the behaviour as described. More details please!

User avatar
joan
Posts: 15092
Joined: Thu Jul 05, 2012 5:09 pm
Location: UK

Re: Accurately controlling a Servo

Sun Apr 14, 2013 4:42 pm

The servo expects timed pulses 50 times per second. If you only send one at irregular intervals the servo behaviour is undefined.

User avatar
jbonnett
Posts: 29
Joined: Mon Jan 21, 2013 2:13 am

Re: Accurately controlling a Servo

Sun Apr 14, 2013 6:06 pm

I don't know what Information to give you apart from if I move the joystick slowly then the servo works perfectly.

The code that works out the servos timing is sent to the servo once every time the joysticks position changes, I'm trying to get this to keep sending the position even if there is no change via the joystick.

Code: Select all

pos_servo = ((0.0000122 * degrees) + 0.0002) # Veriable that calculates new servo position
GPIO.output(10, True) # Turn servo 1 GPIO on
time.sleep(pos_servo) # Wait
GPIO.output(10, False) # Turn servo 1 GPIO off
time.sleep(0.0025 - pos_servo) # Wait
e.g if I run this it also works perfectly

Code: Select all

def move_servo(degrees=90, servo=0): # Function that moves servo position
	pos_servo = ((0.0000122 * degrees) + 0.0002) # Veriable that calculates new servo position
	GPIO.output(10, True) # Turn servo 1 GPIO on
        time.sleep(pos_servo) # Wait
        GPIO.output(10, False) # Turn servo 1 GPIO off
        time.sleep(0.0025 - pos_servo) # Wait

while 1:
        for angle in range(0, 180):
                move_servo(angle)
        for angle in range(0, 180):
                move_servo(180 - angle)

BeeryChisels
Posts: 18
Joined: Sat Apr 13, 2013 7:00 pm

Re: Accurately controlling a Servo

Mon Apr 15, 2013 3:45 am

Hello again jbonnett. Joan is quite right, the control signal needs to be sent repetitively. I think the problem with the fast joystick movement is all that sleeping that's going on within the program, combined with the single output pulses. Part of the definition of a PWM signal is that pulses are sent continuously; it is the mark : space ratio that controls the servo position. Within reason, the repetition rate is often fairly flexible, but must be constant for best results. I don't know which servo you are using, but assuming that the recommended repetition rate is 50Hz, it should work with 'mark' pulses anything between 1ms and 19ms long, leaving a 'space' time of between 19ms and 1ms. You've got to have some space time, which is why the mark pulses cannot be 20ms long for a 50Hz repetition rate. Pulse lengths are often expressed as a percentage of the wavelength (20ms in this case) to allow for different repetition rates, say, between 40 and 60Hz.
I used a servo a long time ago, using a RISC-PC to experiment with it before writing the program. Purely for experimental purposes I hooked the servo up to the User Port and used the keyboard to vary the mark : space ratio and the repetition rate. Naturally, I used Z and X for the mark : space ratio, and @ and ? for the repetition rate. A continuously updated screen display told me what the figures were, so that I could see what the max and min were when the servo stopped working.

Return to “Python”