Page 1 of 1

Controlling servos and motors with a joystick

Posted: Sat Oct 13, 2012 9:10 pm
by ToddFerrante
I've written a Python 3 program which controls servos and motors using input from a joystick. Up to 8 PWM channels can be controlled using just the Raspberry Pi GPIO pins. This program uses code and concepts from multiple sources.

Credits:
I use the servoblaster kernal and driver to output the PWM signals. Details on servoblaster here: http://www.raspberrypi.org/phpBB3/viewt ... 37&t=15011
To interface to servoblaster from within Python, I use commands described here: http://www.raspberrypi.org/phpBB3/viewt ... 32&t=16294
The joystick reading routine comes from here: http://iamtherockstar.com/archive/makin ... joysticks/
The algorithm for applying an exponential control curve and deadband comes from here: http://www.theonerobot.com/resources/ex ... e-function

To load and unload servoblaster, you will need the bash scripts I posted in the first referenced thread, above. If anyone can tell me how to eliminate the need to use these scripts, replacing them with Python commands, I'd much appreciate it.

Currently, the software expects the joystick axis input to vary between -1 and 1. I expect it wouldn't be hard to tweak the code to scale the input of a device that was calibrated differently. (I plan to do this presently.)

I've commented out the print functions that send the output values to the screen. I used these for troubleshooting. Running within Idle3 and printing the values to the screen slows the program down and makes the motor stutter horribly. With the "print"s commented out, control is real time.

To run even faster, I tried running the script straight from the linux prompt, but it doesn't work properly outside of Idle 3. Does anyone know why that is?

Video demonstration here: http://www.youtube.com/watch?v=0ODQY2mDYR0&feature=plcp

Todd F.

Code: Select all

#!/usr/bin/python

import pygame
import os
#from pygame import joystick, event

pygame.init()
#joystick.init()
j = pygame.joystick.Joystick(0)
j.init()
print('Initialized Joystick : %s' % j.get_name())

pulse_max = [240,240,240,240,240,240,240,240]
pulse_neut = [150,150,150,150,150,150,150,150]
pulse_min = [60,60,60,60,60,60,60,60]

os.system("sudo sbload")
sb=open("/dev/servoblaster","w")
for axis in range(0,8):
    sb.write(str(axis)+"="+str(pulse_neut[axis])+"\n")    
    sb.flush()
    #print(str(axis)+"="+str(pulse_neut[axis])+"\n")
print('Initialized Servoblaster')

joy_dead = [.05,.05,.05,.05,.05,.05,.05,.05]
joy_startup = [.05,.05,.05,.05,.05,.05,.05,.05]
joy_exp = [1.5,1.5,1.5,1.5,1.5,1.5,1.5,1.5]

def exp_joy(joy_val, exp, dead, startup):
    live = abs(joy_val)-dead
    if live <= 0:
        return 0
    else:
        return (joy_val/abs(joy_val))*(startup+((1-startup)*pow(live,exp)/pow((1-dead),exp)))

def axis2servo(axis,value):
    if value == 0:
        return pulse_neut[axis]
    elif value > 0:
        return int(pulse_neut[axis]+value*(pulse_max[axis]-pulse_neut[axis]))
    else:
        return int(pulse_neut[axis]+value*(pulse_neut[axis]-pulse_min[axis]))

try:
    while True:
        pygame.event.pump()
        for i in range(0, j.get_numaxes()):
            if j.get_axis(i) != 0.00:
                axis_val = j.get_axis(i)
                curved_val = exp_joy(axis_val,joy_exp[i],joy_dead[i],joy_startup[i])
                servo_pulse = axis2servo(i,curved_val)
                #print('Axis', i,'reads', axis_val, 'exponentialized', curved_val, 'pulse', servo_pulse)
                sb.write(str(i)+"="+str(servo_pulse)+"\n")
                sb.flush()
        for i in range(0, j.get_numbuttons()):
            if j.get_button(i) != 0:
                print('Button %i reads %i' % (i, j.get_button(i)))
except KeyboardInterrupt:
    print('Ending program.')
    j.quit()
    sb.close()
    os.system("sudo sbunload")

Re: Controlling servos and motors with a joystick

Posted: Sat Oct 13, 2012 11:20 pm
by wallarug
I have done this with python as well. See here for my code:
http://www.raspberrypi.org/phpBB3/viewt ... 37&t=19725

I don't know how much help I can be but this is as far as I have gotten.

Re: Controlling servos and motors with a joystick

Posted: Sun Oct 14, 2012 12:07 pm
by rgh
ToddFerrante wrote: To load and unload servoblaster, you will need the bash scripts I posted in the first referenced thread, above. If anyone can tell me how to eliminate the need to use these scripts, replacing them with Python commands, I'd much appreciate it.
Hi Todd, we should really figure out why you are having trouble getting the device node created automatically when you insmod the driver. The instructions I put in the driver source to make udev do that always work fine for me. I guess it doesn't entirely solve your problem though as you'd still need to 'sudo insmod' the driver. You could of course arrange to have the driver loaded automatically on boot up, by adding commands to /etc/rc.local or similar.

Richard

Re: Controlling servos and motors with a joystick

Posted: Mon Oct 22, 2012 12:56 am
by ToddFerrante
Here is a status update on my project. I now have the joystick servo control working over a WiFi link. I'm using Raspberry Pis for both the control station and the robot controller. The control station Pi is hardwired to a wireless router, and has a static IP. The robot controller Pi has a WiFi transmitter connected to the USB port. I have yet to find a way to make the wireless IP address static (nothing I've found on the web actually works), so the router is assigning that IP address.

Each Pi has its own Python program running. The control station is a network client. The station takes joystick input, converts it into pulse width commands for servoblaster, then sends them as UDP packets to the server. The robot controller is a network server. It receives UDP packets from the client, and sends them directly to servoblaster. Since this is a proof of concept at this point, there is no error checking to be sure the packets came through cleanly.

Credits:
For UDP networking I relied heavily on the examples from these two sources:
http://evolt.org/node/60276 This example is easily understood, but has some syntax errors.
http://www.doughellmann.com/PyMOTW/socket/udp.html This example fixes the syntax errors, but has weird stuff in the print commands.
Since I'm working in Python 3, strings must be encoded into bytes to be sent over the network, and decoded on the other end. The clearest explanation I found on how to do this is at the bottom of this page http://stackoverflow.com/questions/1178 ... -interface

A video demonstration is here: http://www.youtube.com/watch?v=B0i1t9Sq0mM&feature=plcp

Finally, the code to make it all work...
To get the control station wired network connection to have a static IP address, I had to edit the /etc/network/interface file like this:

Code: Select all

auto lo
iface lo inet loopback  
#iface eth0 inet dhcp
iface eth0 inet static
address 192.168.1.10
netmask 255.255.255.0
gateway 192.168.1.1

allow-hotplug wlan0
iface wlan0 inet manual
wpa-roam /etc/wpa_supplicant/wpa_supplicant.conf

iface default inet dhcp
The wlan0 section was automatically generated by the stock RPi wireless networking tool, which doesn't seem to let you set a static IP for the WiFi connection. If anyone can point me to a way that works (with Raspbian), I'd appreciate it.

The Python code assumes you have edited your /etc/hosts file so the names used by the server and client programs go to the right IP addresses. Instead of names, the raw IP addresses could be used in the code, but I found it helpful when trouble shooting to be able to type "ping robotcontroller" instead of trying to remember the numbers.

Code: Select all

127.0.0.1	localhost
::1		localhost ip6-localhost ip6-loopback
fe00::0		ip6-localnet
ff00::0		ip6-mcastprefix
ff02::1		ip6-allnodes
ff02::2		ip6-allrouters

127.0.1.1	raspberrypi
192.168.1.1	robotrouter
192.168.1.100	robotcontroller
192.168.1.10	controlstation
At last, here are the server and client programs. I always start the server before starting the client. I'm not sure what would happen if you did this backwards.

joyservorobot.py

Code: Select all

#!/usr/bin/python

import os
import socket

# Set the socket parameters
host = "robotcontroller"
port = 2363
buf = 64
robotaddr = (host,port)

os.system("sudo sbload")
sb=open("/dev/servoblaster","w")
for axis in range(0,8):
    sb.write(str(axis)+"="+str(150)+"\n")    
    sb.flush()
    #print(str(axis)+"="+str(pulse_neut[axis])+"\n")
print('Initialized Servoblaster')

# Create socket
UDPSock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
UDPSock.bind(robotaddr)
print('Created socket')


sbcommand = "nothing"
try:
    while sbcommand != "shutdown robot":
        sbcommandb, addr = UDPSock.recvfrom(buf)
        sbcommand = sbcommandb.decode()
        #print(sbcommand)
        sb.write(sbcommand)
        sb.flush()
    else:
        print('Shutdown command')
except KeyboardInterrupt:
    print('Ending program.')
    j.quit()
    sb.close()
    os.system("sudo sbunload")
    # Close socket
    UDPSock.close()
joyservostation.py

Code: Select all

#!/usr/bin/python

import pygame
import os
import socket
#from pygame import joystick, event

# Set the socket parameters
host = "robotcontroller"
port = 2363
buf = 64
robotaddr = (host,port)

pygame.init()
#joystick.init()
j = pygame.joystick.Joystick(0)
j.init()
print('Initialized Joystick : %s' % j.get_name())

# Create socket
UDPSock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
print('Created socket')

pulse_max = [240,240,240,240,240,240,240,240]
pulse_neut = [150,150,150,150,150,150,150,150]
pulse_min = [60,60,60,60,60,60,60,60]

os.system("sudo sbload")
sb=open("/dev/servoblaster","w")
for axis in range(0,8):
    sbcommand = str(axis)+"="+str(pulse_neut[axis])+"\n"
    #sb.write(sbcommand)    
    #sb.flush()
    UDPSock.sendto(sbcommand.encode(),robotaddr)
    #print(str(axis)+"="+str(pulse_neut[axis])+"\n")
print('Initialized Servoblaster')

joy_dead = [.05,.05,.05,.05,.05,.05,.05,.05]
joy_startup = [.05,.05,.05,.05,.05,.05,.05,.05]
joy_exp = [1.5,1.5,1.5,1.5,1.5,1.5,1.5,1.5]

def exp_joy(joy_val, exp, dead, startup):
    live = abs(joy_val)-dead
    if live <= 0:
        return 0
    else:
        return (joy_val/abs(joy_val))*(startup+((1-startup)*pow(live,exp)/pow((1-dead),exp)))

def axis2servo(axis,value):
    if value == 0:
        return pulse_neut[axis]
    elif value > 0:
        return int(pulse_neut[axis]+value*(pulse_max[axis]-pulse_neut[axis]))
    else:
        return int(pulse_neut[axis]+value*(pulse_neut[axis]-pulse_min[axis]))

try:
    while True:
        pygame.event.pump()
        for i in range(0, j.get_numaxes()):
            if j.get_axis(i) != 0.00:
                axis_val = j.get_axis(i)
                curved_val = exp_joy(axis_val,joy_exp[i],joy_dead[i],joy_startup[i])
                servo_pulse = axis2servo(i,curved_val)
                #print('Axis', i,'reads', axis_val, 'exponentialized', curved_val, 'pulse', servo_pulse)
                sbcommand = str(i)+"="+str(servo_pulse)+"\n"
                #sb.write(sbcommand)
                #sb.flush()
                UDPSock.sendto(sbcommand.encode(),robotaddr)
        for i in range(0, j.get_numbuttons()):
            if j.get_button(i) != 0:
                print('Button %i reads %i' % (i, j.get_button(i)))
except KeyboardInterrupt:
    print('Ending program.')
    j.quit()
    sb.close()
    os.system("sudo sbunload")
    # Close socket
    UDPSock.close()
enjoy,
Todd F.

Re: Controlling servos and motors with a joystick

Posted: Tue Oct 23, 2012 12:18 am
by ToddFerrante
I finally found a solution to making the wireless IP address static. It's in this post :
http://www.raspberrypi.org/phpBB3/viewt ... 85#p180585

I'm changing the wireless IP of robotcontroller to 192.168.1.11
Todd F.

Re: Controlling servos and motors with a joystick

Posted: Sun Nov 18, 2012 9:51 pm
by ToddFerrante
I haven't posted an update in a while because I have been working, unsuccessfully, on getting a usb webcam to stream from my Pi robot controller to my Pi control station. But, I just discovered something that is noteworthy. One of the tools suggested to stream the webcam is netcat. Though it didn't work for the webcam, it is a nifty way to easily transmit commands to servoblaster over a wifi link.

First, install netcat onto both the robot controller and the control station using:

Code: Select all

sudo apt-get install netcat
Keep in mind that I have "robotcontroller" and "controlstation" defined in my hosts file with the static ip addresses of my two Pis.

On the robotcontroller (connected to the motors) do:

Code: Select all

sudo sbload
nc -v -lp 5000 > /dev/servoblaster
The second command activates netcat in listen mode, listening to port 5000 and sending any input to servoblaster.

On the controlstation do:

Code: Select all

nc -v robotcontroller 5000
This activates netcat and sends any typed input directly to port 5000 on robotcontroller.
Now, on controlstation type servoblaster commands, such as:

Code: Select all

1=170
1=190
1=210
1=180
1=150
This series of commands moves servo number 1. When finished, hit crtl-c on each machine to stop netcat.

Todd F.

Re: Controlling servos and motors with a joystick

Posted: Tue Mar 12, 2013 8:08 am
by jniedung
Hello Todd

If the above Sample worked for you you were lucky.
My Tektronix tells me that the Servo Pulses are about 3 to 4 Times longer than usual and my ESC ,
if it will do anything , will do an on or an off but no PWM.
This happens even if I set servo_pulse to a fixed value of 150. Then Pulse is 2.5 ms minimum.

servod from a terminal session works correct - What are these SDL_getaxis.... Messages I see
after starting the Python code ? A Pygame Problem ?

OH I am using a PS 3 Controller via Bluetooth and Pi is wheezy @ 700 MHZ

Regards JoE

Re: Controlling servos and motors with a joystick

Posted: Wed Mar 13, 2013 10:12 pm
by jniedung
Use option --pcm

pygame seems to have problems with pwm mode.

unfortunately there is another flaw.

Servo wont return if stick centers fast (And I am not able to fix it because of these indented block
stuff - LOL) .

No Begins , No Ends - strange

Regards JoE

Willst Du das ein Job erledigt wird mach ihn selbst.

Re: Controlling servos and motors with a joystick

Posted: Thu Mar 14, 2013 7:14 pm
by jniedung
I give up.

Python is rubbish.
Its not even worth reading this silly language,

Who ever created this indentation mechanismen - Well done Guy , you are the greatest..

Sorry to leave you alone , but this sucks

Re: Controlling servos and motors with a joystick

Posted: Fri Mar 15, 2013 4:52 pm
by Frankels
Hi Todd,
Great work on the networking side. I'm trying to accomplish something similar. But before I elaborate: you said you had trouble streaming your webcam. I too encountered this problem but there's an easy fix:
ssh -X ipaddresofwebcambox
cheese &
which starts the webcam viewer program cheese over ssh. This worked right away for me. I did have to set my wifi router to another channel to get it to stream a bit faster. Of course cheese does need to be installed on the webcambox. Maybe you already know this but I know I was looking for more complicated solutions before I encounterred this. Hope it helps.

Now to elaborate: I have a pc with linuxCNC (robot) with steppermotors I want to control over wifi with my desktop PC. The desktop pc has a joystick attached.
Both run ubuntu
So far i can jog the motor with joystickbuttons using emcrsh, but I'm trying to get it to work through halrmt. I'm experimenting with telnet, ssh, pipelining, shellscripts and python...
But I get confused about which processes/scripts to run on which side of the connection and the whole pyhon doing telnet/ssh thing. Maybe thats why you use the UDP approach? You do seem to have more advanced coding skills than me (for now ;) ).
Am i correct in assuming the UDP part takes the joystick parameters and streams them constantly over the network to the robot?