User avatar
raspberrypiguy1
Posts: 379
Joined: Sun Sep 02, 2012 7:01 pm

Wiimote Cwiid - am I disconnected? RPi Electric Skateboard

Mon Dec 26, 2016 9:28 pm

Hi folks,

For a bit of fun I created an electric skateboard that has a Raspberry Pi Zero at its heart. I made a YouTube video on the skateboard and (up until now) it has worked swimmingly! You can view the video here: https://www.youtube.com/watch?v=2WLEur3M8Yk

I was riding the skateboard today (perhaps my sixth outing with it) when the Wiimote that I use to control the board lost connection (never happened before - Pi/Wiimote combo is normally rock solid) and the board continued at its current low speed. I jumped off, ran off the momentum and grabbed the board and powered it down. Very strange. I then went home to see what was up...

The program controlling the ESC and electronics under the board is some object-oriented Python that uses the Cwiid library to talk to the Wiimote. All the code can be found on Github (https://github.com/the-raspberry-pi-guy/skateboard), but the main program is here:

Code: Select all

import pigpio
import time
import cwiid
import os
import sys

from timeout import timeout, TimeoutError

pi = pigpio.pi()
is_debug = "debug" in sys.argv

class Skateboard(object):
	"""An all-powerful skateboard controller"""

	# Constants for values used by class
	motor = 18
	led = 17
	button = 27
	lights_on = 26
	lights_off = 16

	min_speed = 1720
	max_speed = 1100

	servo_smooth = 2
	smooth_sleep = 0.005
	accel_sleep = 0.02
	indicator_lights_on = 0

	# Initial setup of pins and various values
	def __init__(self):
		pi.set_PWM_frequency(Skateboard.motor, 50)
		pi.set_mode(Skateboard.led, pigpio.OUTPUT)
		pi.set_mode(Skateboard.button, pigpio.INPUT)
		pi.set_mode(Skateboard.lights_on, pigpio.OUTPUT)
		pi.set_mode(Skateboard.lights_off, pigpio.OUTPUT)
		pi.set_pull_up_down(Skateboard.button, pigpio.PUD_UP)
		self.__speed = 1500
		self.speed=1500

	@property
	def speed(self):
		return self.__speed

	# Decorator to push speed value to ESC as soon as when changed
	@speed.setter
	def speed(self, value):
		value = max(min(value, Skateboard.min_speed), Skateboard.max_speed)
		while abs(value-self.__speed) > Skateboard.servo_smooth:
			direction = cmp(value, self.__speed)
			self.__speed += direction * Skateboard.servo_smooth
			pi.set_servo_pulsewidth(Skateboard.motor, self.__speed)
			time.sleep(Skateboard.smooth_sleep)
		pi.set_servo_pulsewidth(Skateboard.motor, value)		
		self.__speed = value
		time.sleep(Skateboard.accel_sleep)
	
	# Blinks the ring LED of the power button on electric skateboard
	def blinky(self,times,period):
		for i in range (1,times):
			pi.write(self.led,1)
			time.sleep(period)
			pi.write(self.led,0)
			time.sleep(period)

	# Toggles an Arduino that toggles the neopixels on the bottom of the electric skateboard
	def arduino_trigger(self):
		if Skateboard.indicator_lights_on == 0:
			pi.write(Skateboard.lights_on,1)
			Skateboard.indicator_lights_on = 1
			self.wii.led = 15
		elif Skateboard.indicator_lights_on == 1:
			pi.write(Skateboard.lights_off,1)
			pi.write(Skateboard.lights_on,0)
			Skateboard.indicator_lights_on = 0
			self.wii.led = 0
		time.sleep(0.5) # Let's hope I don't activate this whilst on the board and die from this half second delay

	# Connects to Wiimote with specified mac address
	def connection_process(self):
		connected = False
		while not connected:
			self.blinky(5,0.4)
			try:
				self.wii = cwiid.Wiimote(bdaddr="00:1F:C5:86:3E:85")
				connected = True
				self.blinky(40,0.03)
				self.wii.rpt_mode = cwiid.RPT_BTN
				self.wii.rumble = 1
				time.sleep(1)
				self.wii.rumble = 0
			except RuntimeError:
				pass

	# Controller-skateboard interface
	def run_process(self):
		pi.write(self.led, 1)
		self.get_status()
		if self.status_button:
			self.wii.rumble=1
			time.sleep(2)
			self.wii.rumble=0
			raise RuntimeError("Status Button")
		
		if (self.buttons & cwiid.BTN_A):
			self.arduino_trigger()
				
		if (self.buttons & cwiid.BTN_B):
			self.speed = 1500
			time.sleep(0.5)
		if (self.buttons & cwiid.BTN_DOWN):
			self.speed += 1
		if (self.buttons & cwiid.BTN_UP):
			self.speed -= 1
		if (self.buttons & cwiid.BTN_PLUS):
			Skateboard.accel_sleep += 0.005
			time.sleep(0.5)
			if Skateboard.accel_sleep >= 0.1:
				Skateboard.accel_sleep = 0.1
			print(Skateboard.accel_sleep)
		if (self.buttons & cwiid.BTN_MINUS):
			Skateboard.accel_sleep -= 0.005
			time.sleep(0.5)
			if Skateboard.accel_sleep <= 0:
				Skateboard.accel_sleep = 0
			print(Skateboard.accel_sleep)

	@timeout(0.4)
	def get_status(self):
		self.buttons = self.wii.state['buttons']
		self.status_button = not pi.read(Skateboard.button)

	
### Main Program ###

# Class instance and program run
skate = Skateboard()
skate.blinky(20,0.05)
skate.connection_process()
while True:
	try:
		skate.run_process()
#		print(skate.speed)
	except KeyboardInterrupt:
		raise
	except:
		skate.speed = 1500
		if is_debug:
			raise
		else:
			os.system("poweroff")
At home I was able to replicate the error by pulling the batteries out of the Wiimote, or disconnecting the Pi's Bluetooth dongle: essentially any interruption to the Bluetooth connection and the program will continue to power the board at its current speed - something that is potentially dangerous!

Originally I did have a fail safe in the program that I thought was protecting me from this: the function "get_status". This allegedly pings the Wiimote every cycle using the wii.state command. I then assumed that if I heard back that the Wiimote was still connected and everything was rosy. I used someone else's timeout code to detect if the code hangs for more than 0.4 seconds. The idea being that if I didn't hear back from the Wiimote then the board's speed would be killed. The timeout code came from here: http://stackoverflow.com/questions/2281 ... -to-finish

Subsequent analysis shows that the wii.state command does not always give you a live reading of the state of the Wiimote - instead it gives you the last available one. So if I have disconnected then my code just continues on!

My question is: how can I make it so that my Python script sets the speed to 0 and shuts down if the Bluetooth connection between Pi and Wiimote is broken? Is there a way to ping a Bluetooth device very quickly and see if it is still operational? How do I kill the speed as soon as the Bluetooth connection has dropped? How do I stop this issue in future?

On a last note: the Cwiid library does have a request_status function that hangs if the Wiimote is not detected - perfect, right?! Not quite. When integrated in my code it does work, however Python spends a while processing that particular part and thus often it misses key presses on the Wiimote: stopping me accelerating, stopping, turning lights on etc.

Can anyone help?!

Matt
The Raspberry Pi Guy
Matt, The Raspberry Pi Guy YouTube channel, author of Learn Robotics with Raspberry Pi, available now: http://mybook.to/raspirobots, Computer Science & Electronics Undergraduate at The University of Edinburgh

User avatar
paddyg
Posts: 2528
Joined: Sat Jan 28, 2012 11:57 am
Location: UK

Re: Wiimote Cwiid - am I disconnected? RPi Electric Skateboa

Tue Dec 27, 2016 8:29 pm

I haven't looked in much detail at the cwiid module (or even if there are LEDs on the wiimote that are settable and checkable from the wii) but could you do something along the lines of:
set a (harmless) LED to on
check the state of the LED
set the LED off
check the state of the LED
etc.
So even if you keep the pedal flat on the floor to get up Castle Hill your program will know that communication is still going on because something is changing.

Alternatively could you run the slow request_state function in another thread or does it block your control message state requests?
also https://groups.google.com/forum/?hl=en-GB&fromgroups=#!forum/pi3d

User avatar
raspberrypiguy1
Posts: 379
Joined: Sun Sep 02, 2012 7:01 pm

Re: Wiimote Cwiid - am I disconnected? RPi Electric Skateboa

Tue Dec 27, 2016 10:36 pm

I come back to this post with good news! I have 100% fixed the issue.

So I actually spent a long time trawling through the Cwiid library and wracking my brain to see if I could get a live report on a Wiimote's status - but with no success. Things like request_status were simply too slow and cumbersome, whereas another idea was to monitor the accelerometer values and if they are too similar than probably the connection has dropped out. I didn't like the latter idea because that has the potential to generate some false positives: for example, when I am standing still and just demonstrating it.

I proceeded to have another gander around the net and ask a few people: I eventually landed on a linux command called l2ping. It is an excellent little tool that just pings Bluetooth devices. If one uses the command:

Code: Select all

sudo l2ping -c 1 -t 1  "some mac address"
That particularly device will be pinged relatively quickly. If you read the output of that command into a variable and see that there is a "100% loss" in there somewhere, then chances are: your Wiimote has disconnected or is no longer in range. Similarly, if the output of that command is just null, then the Bluetooth dongle has stopped functioning. All-in-all this works really well, but the only way to solidly implement it was through a bit of threading and so I learnt how to do that in Python and worked on my code again. In the end I came up with the following solution, and now the board is safe to ride again. Github: https://github.com/the-raspberry-pi-guy ... teboard.py

Code: Select all

import pigpio
import time
import cwiid
import sys
import threading
import subprocess

pi = pigpio.pi()
is_debug = "debug" in sys.argv

# Global constants
motor = 18
led = 17
button = 27
lights_on = 26
lights_off = 16

wiimote_bluetooth = "00:1F:C5:86:3E:85"
powerdown = ["sudo", "shutdown", "now"]

stop_val = False

class Skateboard(object):
	""" An all-powerful skateboard controller """

	# Constants for values used by class
	min_speed = 1720
	max_speed = 1100

	servo_smooth = 2
	smooth_sleep = 0.005
	accel_sleep = 0.015
	indicator_lights_on = 0

	# Initial setup of pins and various values
	def __init__(self):
		pi.set_PWM_frequency(motor, 50)
		pi.set_mode(led, pigpio.OUTPUT)
		pi.set_mode(button, pigpio.INPUT)
		pi.set_mode(lights_on, pigpio.OUTPUT)
		pi.set_mode(lights_off, pigpio.OUTPUT)
		pi.set_pull_up_down(button, pigpio.PUD_UP)
		self.__speed = 1500
		self.speed = 1500

	@property
	def speed(self):
		return self.__speed

	# Decorator to push speed value to ESC as soon as when changed
	@speed.setter
	def speed(self, value):
		value = max(min(value, Skateboard.min_speed), Skateboard.max_speed)
		while abs(value-self.__speed) > Skateboard.servo_smooth:
			direction = cmp(value, self.__speed)
			self.__speed += direction * Skateboard.servo_smooth
			pi.set_servo_pulsewidth(motor, self.__speed)
			time.sleep(Skateboard.smooth_sleep)
		pi.set_servo_pulsewidth(motor, value)		
		self.__speed = value
		time.sleep(Skateboard.accel_sleep)
	
	# Blinks the ring LED of the power button on electric skateboard
	def blinky(self,times,period):
		for i in range (1,times):
			pi.write(led,1)
			time.sleep(period)
			pi.write(led,0)
			time.sleep(period)

	# Toggles an Arduino that toggles the neopixels on the bottom of the electric skateboard
	def arduino_trigger(self):
		if Skateboard.indicator_lights_on == 0:
			pi.write(lights_on,1)
			Skateboard.indicator_lights_on = 1
			self.wii.led = 15
		elif Skateboard.indicator_lights_on == 1:
			pi.write(lights_off,1)
			pi.write(lights_on,0)
			Skateboard.indicator_lights_on = 0
			self.wii.led = 0
		time.sleep(0.5) # Let's hope I don't activate this whilst on the board and die from this half second delay

	# Connects to Wiimote with specified mac address
	def connection_process(self):
		connected = False
		while not connected:
			self.blinky(5,0.4)
			try:
				self.wii = cwiid.Wiimote(bdaddr = wiimote_bluetooth)
				connected = True
				self.blinky(40,0.03)
				self.wii.rpt_mode = cwiid.RPT_BTN
				self.wii.rumble = 1
				time.sleep(1)
				self.wii.rumble = 0
			except RuntimeError:
				pass

	# Controller-skateboard interface
	def run_process(self):
		global stop_val
		pi.write(led, 1)
		while (stop_val == False):
			self.get_status()
			if self.status_button:
				self.wii.rumble=1
				time.sleep(2)
				self.wii.rumble=0
				raise RuntimeError("Status Button")
		
			if (self.buttons & cwiid.BTN_A):
				self.arduino_trigger()
				
			if (self.buttons & cwiid.BTN_B):
				self.speed = 1500
				time.sleep(0.5)
			if (self.buttons & cwiid.BTN_DOWN):
				self.speed += 1
			if (self.buttons & cwiid.BTN_UP):
				self.speed -= 1
			if (self.buttons & cwiid.BTN_PLUS):
				Skateboard.accel_sleep += 0.005
				time.sleep(0.5)
				if Skateboard.accel_sleep >= 0.1:
					Skateboard.accel_sleep = 0.1
				print(Skateboard.accel_sleep)
			if (self.buttons & cwiid.BTN_MINUS):
				Skateboard.accel_sleep -= 0.005
				time.sleep(0.5)
				if Skateboard.accel_sleep <= 0:
					Skateboard.accel_sleep = 0
				print(Skateboard.accel_sleep)
		self.speed = 1500 #If the board defaults, set the speed to neutral

	def get_status(self):
		self.buttons = self.wii.state['buttons']
		self.status_button = not pi.read(button)

class wiimote_watcher(threading.Thread):
	""" A wiimote checking thread class """

	bluetooth_ping = ["sudo", "l2ping", "-c", "1", "-t", "1", wiimote_bluetooth]
		
	def run(self):
		while True:
			self.wiimote_check()
			time.sleep(0.1)

	def try_comms(self):
        	command = subprocess.Popen(wiimote_watcher.bluetooth_ping, stdout=subprocess.PIPE).communicate()[0]
        	return command

	def motor_off(self):
		global stop_val
        	stop_val = True # Causes main thread loop to stop working and speed to default

	def shutdown(self):
		self.motor_off()
        	if is_debug:
			print "OFF"
		else:
			subprocess.call(powerdown)

	def wiimote_check(self):
		try:
			output = self.try_comms()
			print output
			if (("100% loss") in output) or (output == ""): # If 100% packets lost: wiimote died. If output is null: bluetooth dongle died
				self.shutdown()
		except:
			self.shutdown()			

###

def main():
	# Class instance and program run
	skate = Skateboard()
	skate.blinky(20,0.05)
	skate.connection_process()
	# Wiimote checker thread
	checker = wiimote_watcher()
	checker.daemon = True
	checker.start()
	try:
		skate.run_process()
	except KeyboardInterrupt:
		raise
	except:
		skate.speed = 1500
		if is_debug:
			raise
		else:
			subprocess.call(powerdown)

if __name__ == "__main__":
	main()
Matt, The Raspberry Pi Guy YouTube channel, author of Learn Robotics with Raspberry Pi, available now: http://mybook.to/raspirobots, Computer Science & Electronics Undergraduate at The University of Edinburgh

bleh20
Posts: 1
Joined: Thu Feb 01, 2018 3:29 am

Re: Wiimote Cwiid - am I disconnected? RPi Electric Skateboard

Thu Feb 01, 2018 3:39 am

i thought you should know since you are using this command as a safety feature...

I tested it myself. It does ping devices that are not connect that are just active. I am able to just make the wiimote active but not connect and get a ping response. I also tested just with my phone's bluetooth on but not connected and the ping returns a response.

I also am looking for the disconnected status.

Return to “Python”