Crawler phase 1 complete.

Crawler is finally working:

The problem was the cwiid python library only recognises the 1st edition Wii controllers – those without Wii motionplus.  You can get these originals from ebay.  Here’s the code.

#!/usr/bin/env python

import RPi.GPIO as GPIO # Import the GPIO Library
import cwiid
import time
import os


# Set the GPIO modes
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)

# Set variables for the GPIO motor pins
pinMotorAForwards = 10
pinMotorABackwards = 9
pinMotorBForwards = 8
pinMotorBBackwards = 7

# Set additional variables to support L293D bodge
pinMotorA1 = 4
pinMotorA2 = 27
pinMotorB2 = 22
pinMotorB1 = 23

# Set variables for the line detector GPIO pin
pinLineFollower = 25

# Define GPIO pins to use on the Pi
pinTrigger = 17
pinEcho = 18

# Set variable for the LED pin
pinLED1 = 5
pinLED2 = 6

# How many times to turn the pin on and off each second
Frequency = 20
# How long the pin stays on each cycle, as a percent
DutyCycleA = 100
DutyCycleB = 100
# Setting the duty cycle to 0 means the motors will not turn
Stop = 0

# Set the GPIO Pin mode to be Output
GPIO.setup(pinMotorAForwards, GPIO.OUT)
GPIO.setup(pinMotorABackwards, GPIO.OUT)
GPIO.setup(pinMotorBForwards, GPIO.OUT)
GPIO.setup(pinMotorBBackwards, GPIO.OUT)

GPIO.setup(pinMotorA1, GPIO.OUT)
GPIO.setup(pinMotorA2, GPIO.OUT)
GPIO.setup(pinMotorB1, GPIO.OUT)
GPIO.setup(pinMotorB2, GPIO.OUT)

GPIO.output(pinMotorA1, False)
GPIO.output(pinMotorA2, False)
GPIO.output(pinMotorB1, False)
GPIO.output(pinMotorB2, False)

# Set the pinLineFollower pin as an input so its value can be read
GPIO.setup(pinLineFollower, GPIO.IN)

def StopMotors():
    pwmMotorAForwards.ChangeDutyCycle(Stop)
    pwmMotorABackwards.ChangeDutyCycle(Stop)
    pwmMotorBForwards.ChangeDutyCycle(Stop)
    pwmMotorBBackwards.ChangeDutyCycle(Stop)
    GPIO.output(pinLED1, False)
    GPIO.output(pinLED2, False)

    GPIO.output(pinMotorA1, False)
    GPIO.output(pinMotorA2, False)
    GPIO.output(pinMotorB1, False)
    GPIO.output(pinMotorB2, False)

def Backwards():
    pwmMotorAForwards.ChangeDutyCycle(DutyCycleA)
    pwmMotorABackwards.ChangeDutyCycle(Stop)
    pwmMotorBForwards.ChangeDutyCycle(DutyCycleB)
    pwmMotorBBackwards.ChangeDutyCycle(Stop)
    GPIO.output(pinLED1, False)
    GPIO.output(pinLED2, False)

    GPIO.output(pinMotorA1, False)
    GPIO.output(pinMotorA2, True)
    GPIO.output(pinMotorB1, False)
    GPIO.output(pinMotorB2, True)

def Forwards():
    pwmMotorAForwards.ChangeDutyCycle(Stop)
    pwmMotorABackwards.ChangeDutyCycle(DutyCycleA)
    pwmMotorBForwards.ChangeDutyCycle(Stop)
    pwmMotorBBackwards.ChangeDutyCycle(DutyCycleB)
    GPIO.output(pinLED1, True)
    GPIO.output(pinLED2, True)

    GPIO.output(pinMotorA1, True)
    GPIO.output(pinMotorA2, False)
    GPIO.output(pinMotorB1, True)
    GPIO.output(pinMotorB2, False)

def Right():
    print("Right")
    pwmMotorAForwards.ChangeDutyCycle(Stop)
    pwmMotorABackwards.ChangeDutyCycle(DutyCycleA * 0.3)
    pwmMotorBForwards.ChangeDutyCycle(DutyCycleB * 0.3)
    pwmMotorBBackwards.ChangeDutyCycle(Stop)
    GPIO.output(pinLED1, True)
    GPIO.output(pinLED2, False)

    GPIO.output(pinMotorA1, True)
    GPIO.output(pinMotorA2, False)
    GPIO.output(pinMotorB1, False)
    GPIO.output(pinMotorB2, True)

def BLeft():
    print("Right")
    pwmMotorAForwards.ChangeDutyCycle(DutyCycleA * 0.3)
    pwmMotorABackwards.ChangeDutyCycle(Stop)
    pwmMotorBForwards.ChangeDutyCycle(DutyCycleB)
    pwmMotorBBackwards.ChangeDutyCycle(Stop)
    GPIO.output(pinLED1, True)

    GPIO.output(pinMotorA1, True)
    GPIO.output(pinMotorA2, False)
    GPIO.output(pinMotorB1, False)
    GPIO.output(pinMotorB2, True)

def FLeft():
    print("Right")
    pwmMotorAForwards.ChangeDutyCycle(Stop)
    pwmMotorABackwards.ChangeDutyCycle(DutyCycleA * 0.3)
    pwmMotorBForwards.ChangeDutyCycle(Stop)
    pwmMotorBBackwards.ChangeDutyCycle(DutyCycleB)
    GPIO.output(pinLED1, True)

    GPIO.output(pinMotorA1, True)
    GPIO.output(pinMotorA2, False)
    GPIO.output(pinMotorB1, False)
    GPIO.output(pinMotorB2, True)

def Left():
    print("Left")
    pwmMotorAForwards.ChangeDutyCycle(DutyCycleA * 0.3)
    pwmMotorABackwards.ChangeDutyCycle(Stop)
    pwmMotorBForwards.ChangeDutyCycle(Stop)
    pwmMotorBBackwards.ChangeDutyCycle(DutyCycleB * 0.3)
    GPIO.output(pinLED1, False)
    GPIO.output(pinLED2, True)

    GPIO.output(pinMotorA1, False)
    GPIO.output(pinMotorA2, True)
    GPIO.output(pinMotorB1, True)
    GPIO.output(pinMotorB2, False)

def BRight():
    print("Left")
    pwmMotorAForwards.ChangeDutyCycle(DutyCycleA)
    pwmMotorABackwards.ChangeDutyCycle(Stop)
    pwmMotorBForwards.ChangeDutyCycle(DutyCycleB * 0.3)
    pwmMotorBBackwards.ChangeDutyCycle(Stop)
    GPIO.output(pinLED1, False)

    GPIO.output(pinMotorA1, False)
    GPIO.output(pinMotorA2, True)
    GPIO.output(pinMotorB1, True)
    GPIO.output(pinMotorB2, False)

def FRight():
    print("Left")
    pwmMotorAForwards.ChangeDutyCycle(Stop)
    pwmMotorABackwards.ChangeDutyCycle(DutyCycleA)
    pwmMotorBForwards.ChangeDutyCycle(Stop)
    pwmMotorBBackwards.ChangeDutyCycle(DutyCycleB * 0.3)
    GPIO.output(pinLED1, False)

    GPIO.output(pinMotorA1, False)
    GPIO.output(pinMotorA2, True)
    GPIO.output(pinMotorB1, True)
    GPIO.output(pinMotorB2, False)


def do_WiiRemote():
    global speed, DutyCycleA, DutyCycleB

    #connecting to the wiimote. This allows several attempts
    # as first few often fail.
    print 'Press 1+2 on your Wiimote now...'
    wm = None
    while not wm:
        try:
            LED1.ChangeDutyCycle(50)
            wm=cwiid.Wiimote()

        except RuntimeError:
            continue

    LED1.ChangeDutyCycle(0)
    LED1.stop()

    #set wiimote to report button presses and accelerometer state
    wm.rpt_mode = cwiid.RPT_BTN | cwiid.RPT_ACC

    #turn on led to show connected
    wm.led = 1

    buttons = wm.state['buttons']
    speed = 100
    DutyCycleA = speed
    DutyCycleB = speed
    moving = False

    #--------------------------------------------------------------------------
    # There are official cwiid numbers for the following - I need to

    WII_BTN_2 = 1              # cwiid.BTN_2
    WII_BTN_1 = 2              # cwiid.BTN_1
    WII_BTN_B = 4              # cwiid.BTN_B
    WII_BTN_A = 8              # cwiid.BTN_A
    WII_BTN_MINUS = 16         # cwiid.BTN_MINUS
    WII_BTN_HOME = 128
    WII_BTN_LEFT = 256
    WII_BTN_RIGHT = 512
    WII_BTN_DOWN = 1024
    WII_BTN_UP = 2048
    WII_BTN_PLUS = 4096        # cwiid.BTN_PLUS
                               # cwiid.NUNCHUK_BTN_C
                               # cwiid.NUNCHUK_BTN_Z


    while not(buttons == WII_BTN_HOME + WII_BTN_A + WII_BTN_B):
        buttons = wm.state['buttons']
        print("Buttons: " + str(buttons))
        if (buttons == WII_BTN_A + WII_BTN_B + WII_BTN_2): # Halt
            wm.led = 2
            print ("Halting Raspberry Pi...")
            GPIO.cleanup()
            bashCommand = ("sudo halt")
            print "echo:"+bashCommand
            os.system(bashCommand)
        elif (buttons & cwiid.BTN_1):
            print("BTN_1")
        elif (buttons & cwiid.BTN_2):
            print("BTN_2")
        elif (buttons & cwiid.BTN_A): # Shoot!!! :o)
            print("BTN_A")
            StopMotors()
        elif (buttons & cwiid.BTN_B): # Fire Missile!!! :o)
            print("BTN_B")
        elif (buttons == WII_BTN_UP):
            print("BTN_UP")
            Forwards()
            moving = True
        elif (buttons == WII_BTN_DOWN):
            print("BTN_DOWN")
            Backwards()
            moving = True
        elif (buttons == WII_BTN_LEFT):
            print("BTN_LEFT")
            Left()
            moving = True
        elif (buttons == WII_BTN_RIGHT):
            print("BTN_RIGHT")
            Right()
            moving = True
        elif (buttons == WII_BTN_UP + WII_BTN_RIGHT):
            FRight()
            moving = True
        elif (buttons == WII_BTN_UP + WII_BTN_LEFT):
            FLeft()
            moving = True
        elif (buttons == WII_BTN_DOWN + WII_BTN_RIGHT):
            BRight()
            moving = True
        elif (buttons == WII_BTN_DOWN + WII_BTN_LEFT):
            BLeft()
            moving = True
        elif (buttons & cwiid.BTN_PLUS):
            print("BTN_PLUS")
            if speed <= 90: speed = speed + 10 else: speed = 100 print("Speed: " + str(speed)) DutyCycleA = speed DutyCycleB = speed elif (buttons & cwiid.BTN_MINUS): print("BTN_MINUS") if speed >= 10:
                speed = speed - 10
            else:
                speed = 0
            print("Speed: " + str(speed))
            DutyCycleA = speed
            DutyCycleB = speed
        elif (buttons & cwiid.NUNCHUK_BTN_C): #
            print("NUNCHUK_BTN_C")
        elif (buttons & cwiid.NUNCHUK_BTN_Z): #
            print("NUNCHUK_BTN_Z")
        else:
            print("Nothing...")
            if moving == True:
                moving = False
                StopMotors()

        time.sleep(0.2)

    GPIO.cleanup()



# Set the GPIO to software PWM at 'Frequency' Hertz
pwmMotorAForwards = GPIO.PWM(pinMotorAForwards, Frequency)
pwmMotorABackwards = GPIO.PWM(pinMotorABackwards, Frequency)
pwmMotorBForwards = GPIO.PWM(pinMotorBForwards, Frequency)
pwmMotorBBackwards = GPIO.PWM(pinMotorBBackwards, Frequency)

# Start the software PWM with a duty cycle of 0 (i.e. not moving)
pwmMotorAForwards.start(Stop)
pwmMotorABackwards.start(Stop)
pwmMotorBForwards.start(Stop)
pwmMotorBBackwards.start(Stop)

# Set pins as output and input
GPIO.setup(pinTrigger, GPIO.OUT) # Trigger
GPIO.setup(pinEcho, GPIO.IN) # Echo

# Set the LED Pin mode to be Output
GPIO.setup(pinLED1, GPIO.OUT)
GPIO.setup(pinLED2, GPIO.OUT)

LED1Frequency = 2
LED2Frequency = 2

LED1DutyCycle = 0
LED2DutyCycle = 0

LED1 = GPIO.PWM(pinLED1, LED1Frequency)
LED2 = GPIO.PWM(pinLED2, LED2Frequency)

LED1.start(LED1DutyCycle)
LED2.start(LED2DutyCycle)

# Set trigger to False (Low)
GPIO.output(pinTrigger, False)

# Allow module to settle
time.sleep(0.5)


try:
    do_WiiRemote()

# If you press CTRL+C, cleanup and stop
except KeyboardInterrupt:
    # Reset GPIO settings
    GPIO.cleanup()


So all that remained was to get it to start at boot time.  This set of instructions works perfectly for me.

See previous post for BOM etc

Next time I’m bored or frustrated, I’ll have a go at updating cwiid to recognise newer versions of the Wii controller.


Just spotted this made it onto Adafruit’s blog.

aide-mémoire

As a Father’s Day present from me to me, I’ve just bought a PX4FLOW from ebay.  Reik Hua’s quad can hover until bored or battery discharge with one of these.  Mine’s on a slow-boat from China so could take up to a month to arrive.

When it does, the LEDDAR will go into safe-keeping – not retired, just pending the arrival of its sibling LiDAR Scance Sweep.

Beyond saving me from terminal boredom syndrome (TBS), the PX4FLOW will give me stuff to do over the summer and motivation.  Primarily it gives me  high precision Z axis height and X and Y axis velocities.

So the list of stuff to do for the summer includes

  • Fuse X,Y velocoties with integrated acceleometer velocities from long term low drift hover
  • add distance / direction PIDs with integrated PX4FLOW velocities for X and Y, and it’s build in sonar height for Z axis – this should stop the lurge.
  • get the compass working just for additional yaw stability to start with, though this can obviously be extended to provide orientation.
  • Play with fixed height, horizontal movement flight plans
  • Have a go at flying in circles – requires angle PID to provide a constant port/starboard angle combined with a fore/aft constant velocity

Independently

  • get DHCP / udhcp working on latest Jessie Zoe WAP
  • get Wiimote working for crawler robot – this will probably be my test platform for the Scan Sweep