48. Robotics with PiBot. III - Running the Test Programs

12,000 page views!!
We're nearly at the point where we can start doing some exciting robotics!  There are currently four test programs for the PiBot - python programs which put the current system through its paces, to make sure all the hardware is working as it should. 

Firstly here is a listing of the firmware, which has to be running on the Bot Board's Arduino:

	//
	// PiBot arduino code - PiBot_Firmware_Alpha
	// James Torbett 2014
	// v. 0.1
	// EARLY ALPHA RELEASE
	//
	// Todo:
	// ADC implementation
	// Temperature (thermistor)
	// Voltages
	// Neopixel display
	// Wheel odometry
	// Better stepper timings
	// Serial terminal
	// i2c interface
	#include <Adafruit_NeoPixel.h>
	#include <Servo.h>
	#include <NewPing.h>
	#include <SPI.h>
	#define MAX_DISTANCE 200 // Maximum distance we want to ping for
	// (in cm). Maximum sensor distance is rated at 400-500cm.
	#define SPI_IDLE 0 // initial state of SPI bus
	#define SPI_READ 1 // next byte is a READ from here
	#define SPI_WRITE 2 // next byte is a WRITE to here
	// vars to hold register value and write data. Read data is
	// passed directly out on next clock cycle.
	byte spiRegister = 0;
	byte spiData = 0;
	// pin definitions
	byte pin_motor1dir = 2; // direction of motor 1
	byte pin_motor2dir = 4; // direction of motor 2
	byte pin_motor1pwm = 3; // PWM (speed) of motor 1
	byte pin_motor2pwm = 5; // PWM (speed) of motor 2
	byte pin_stepperDir = 7; // direction of stepper motor
	byte pin_stepperStep = 6; // cycle this pin to perform 1/32 of
	// step
	byte pin_stepperDisable = 8; // low = stepper enable,
	// high = disable to save power
	byte pin_neopixelData = 14; // neopixel data in pin
	byte pin_uSoundTrig = 16; // trigger pin of the ultrasound
	// (ping) module
	byte pin_uSoundEcho = 15; // echo pin of the ultrasound module
	byte pin_servoData = 17; // pin for the servo data
	byte spiReceived = 0; // flag to say we've received an SPI byte
	byte spiByte = 0; // value of the byte received over SPI
	const int SPI_BUFFER_SIZE = 128; // AB: This may need to be
	// a power of 2 for the producer consumer algorithm to work
	volatile unsigned int gSpiProduceCount = 0;
	volatile unsigned int gSpiConsumeCount = 0;
	volatile byte gSpiBuffer[ SPI_BUFFER_SIZE ];
	volatile byte gSpiResult = 0;
	volatile byte gSpiClash = 0;
	unsigned int sonarInterval = 100; // number of loops per sonar
	// measurement
	unsigned int stepperInterval = 100; // number of loops per
	// stepper step
	unsigned int neoPixelInterval = 1000; // number of loops per
	// neopixel array update
	byte stepperValue = 0; // increments. Bit 0 used for stepper
	// pin value.
	// internal data registers
	byte motor1dir = 0;
	int motor2dir = 0;
	byte motor1pwm = 0;
	byte motor2pwm = 0;
	int servoPos = 0;
	byte stepperSpeed = 0;
	byte stepperDir = 0;
	int uSoundDistance = 0;
	const int NUM_NEOPIXELS = 8;
	byte neoPixelData[51] = { 0 };
	// count of hex 0x55 character received on SPI (01010101)
	// if this is received 3 times in a row, reset the SPI state
	// machine
	byte spi55count = 0;
	unsigned int controlCounter = 0;
	byte spiState = SPI_IDLE;
	// define some library objects
	Adafruit_NeoPixel strip = Adafruit_NeoPixel(NUM_NEOPIXELS,
	// pin_neopixelData, NEO_GRB + NEO_KHZ800);
	Servo tiltServo;
	NewPing sonar(pin_uSoundTrig, pin_uSoundEcho, MAX_DISTANCE);
	// NewPing setup of pins and maximum distance.
	void setup()
	// setup: set pin modes and any other associated stuff
	{
	Serial.begin(9600);
	pinMode(pin_motor1dir, OUTPUT);
	pinMode(pin_motor2dir, OUTPUT);
	pinMode(pin_motor1pwm, OUTPUT);
	pinMode(pin_motor2pwm, OUTPUT);
	pinMode(pin_stepperDir, OUTPUT);
	pinMode(pin_stepperStep, OUTPUT);
	pinMode(pin_stepperDisable, OUTPUT);
	pinMode(pin_neopixelData, OUTPUT);
	// start the servo
	tiltServo.attach(pin_servoData);
	// start SPI
	enableSPI();
	}
	ISR (SPI_STC_vect)
	// SPI interrupt routine, activates when a byte is received.
	{
	byte c = SPDR; // grab byte from SPI Data Register
	if ( gSpiProduceCount - gSpiConsumeCount == SPI_BUFFER_SIZE )
	{
	// SPI buffer is full
	gSpiClash = 1;
	}
	else
	{
	gSpiBuffer[ gSpiProduceCount % SPI_BUFFER_SIZE ] = c;
	gSpiProduceCount++;
	}
	SPDR = gSpiResult;
	} // end of interrupt routine SPI_STC_vect
	void enableSPI()
	// enable the SPI bus as slave device (PI is master)
	{
	// have to send on master in, *slave out*
	pinMode(MISO, OUTPUT);
	// turn on SPI in slave mode
	SPCR |= _BV(SPE);
	// Set SPI mode
	SPI.setDataMode( SPI_MODE0 );
	// now turn on interrupts
	SPI.attachInterrupt();
	}
	void controlMotors(void)
	// set the motor drives to values defined in the variables
	{
	digitalWrite(pin_motor1dir, motor1dir);
	analogWrite(pin_motor1pwm, motor1pwm);
	digitalWrite(pin_motor2dir, motor2dir);
	analogWrite(pin_motor2pwm, motor2pwm);
	}
	void controlServo(void)
	// write to the servo
	{
	tiltServo.write(servoPos);
	}
	// toggle the stepper pin
	void doStep(void)
	{
	if (stepperSpeed > 0)
	{
	stepperInterval = 256 - stepperSpeed;
	digitalWrite(pin_stepperDisable, 0);
	digitalWrite(pin_stepperDir, stepperDir);
	digitalWrite(pin_stepperStep, stepperValue & 0x01);
	stepperValue++;
	}
	else
	{
	digitalWrite(pin_stepperDisable, 1);
	}
	}
	void readDistance()
	// read the distance reported by the ultrasound module.
	// make sure to leave at least 50mS between calls to this
	{
	unsigned int uS = sonar.ping();
	uSoundDistance = uS / US_ROUNDTRIP_CM;
	}
	void processSPI(void)
	{
	// finished clocking in a byte
	// process the sync byte regardless of state
	if(spiByte == 0x55)
	{
	//Serial.println( "Sync byte" );
	// increment sync counter
	spi55count++;
	if (spi55count > 2)
	{
	// go back to the idle state
	spiState = SPI_IDLE;
	spiReceived = 0;
	spi55count = 0; // Reset sync conter
	return; // yes, it's an unconditional jump
	// out of subroutine
	}
	}
	if (spiState == SPI_IDLE)
	{
	if (spiByte < 60)
	{
	// Write operation
	spiState = SPI_WRITE;
	spiRegister = spiByte;
	}
	else if (spiByte >= 100)
	{
	spiState = SPI_READ;
	spiRegister = spiByte;
	processSPIRead();
	}
	}
	else if (spiState == SPI_WRITE)
	{
	// we've got a data value now
	spiData = spiByte;
	processRegister();
	spiState = SPI_IDLE;
	}
	else if (spiState == SPI_READ)
	{
	// SPDR data register will have been clocked out now.
	// Return to idle.
	spiState = SPI_IDLE;
	}
	spiReceived = 0;
	}
	void processSPIRead()
	{
	// only one case for now: to read the ultrasound distance.
	switch (spiRegister)
	{
	case 100:
	gSpiResult = uSoundDistance;
	break;
	default:
	break;
	}
	}
	void processRegister()
	{
	switch(spiRegister)
	{
	case 01:
	motor1dir = spiData;
	break;
	case 02:
	motor2dir = spiData;
	break;
	case 03:
	motor1pwm = spiData;
	break;
	case 04:
	motor2pwm = spiData;
	break;
	case 05:
	stepperDir = spiData;
	break;
	case 06:
	stepperSpeed = spiData;
	break;
	case 07:
	servoPos = spiData;
	break;
	case 8 ... 59:
	neoPixelData[spiRegister-8] = spiData;
	// wow departure from ANSI-C
	break;
	default:
	break;
	}
	}
	void updateNeoPixel()
	{
	// todo: actually copy the register data into the
	// neopixel strip
	for ( int pixelIdx = 0; pixelIdx < NUM_NEOPIXELS; pixelIdx++ )
	// sizeof( neoPixelData ) / 3; pixelIdx++ )
	{
	strip.setPixelColor( pixelIdx, neoPixelData[ 3*pixelIdx ],
	neoPixelData[ 3*pixelIdx+1 ], neoPixelData[ 3*pixelIdx+2 ] );
	}
	strip.show();
	}
	void loop(void)
	// main loop
	{
	// do stuff.
	controlCounter++;
	if ( gSpiProduceCount - gSpiConsumeCount > 0 )
	{
	spiByte = gSpiBuffer[ gSpiConsumeCount % SPI_BUFFER_SIZE ];
	spiReceived = 1;
	gSpiConsumeCount++;
	}
	if (spiReceived) processSPI();
	if ( gSpiClash )
	{
	//Serial.println( "Serial clash occured" );
	gSpiClash = 0;
	}
	controlMotors();
	controlServo();
	//Serial.println( controlCounter );
	// only read distance every sonarInterval loops
	if((controlCounter % neoPixelInterval) == 0)
	{
	updateNeoPixel();
	}
	// only read distance every sonarInterval loops
	if((controlCounter % sonarInterval) == 0)
	{
	readDistance();
	}
	// only step stepper based on interval (derived from speed)
	if((controlCounter % stepperInterval) == 0)
	{
	doStep();
	}
	}

view raw PiBot_Firmware_Alpha hosted with ❤ by GitHub

Notice that I have embedded these code listings from my Gist account on GitHub, which is where the open source software from the PiBot Team is published. These are my versions of the PiBot Team's software. Hopefully my future versions, on following Blog posts, will be advanced versions of these. As these are embedded, any changes I make to these on GitHub will be reflected in these listings, so I must be careful to generate new Gists.
The four python programs are:
1. drive_in_square.py,
2. monitor_ultrasonic.py,
3. robot_teleop.py and 
4. test_pibot_hardware.py.  
The code listings that I will give here contain some small adjustments which I made to suit my particular needs.

drive_in_square.py
This test program allows the PiBot to proceed in a straight line for a fixed time, make a right-angled turn, do a straight line, do a right angle, do a straight line and do a right angle - making a square shaped track.  The program continues indefinitely, so it has to be interrupted by performing a Ctrl-Z on the Pi's remote terminal on the PC, followed by touching together the two flying leads on the PiBot to do an Arduino reset. 

You've seen this program in action in the last post, but here's the version with a shortened square:

OK - it's not quite a square - when I changed the speed, I should have adjusted the delay times too - anyway you get the gist! (where have I heard that word before?).

Here's the code:

	#! /usr/bin/env python
	# drive_in_square.py
	import time
	import pibot
	import sys
	bot = pibot.PiBot()
	# speed = 255
	speed = 64
	if len( sys.argv ) > 1:
	speed = int( sys.argv[ 1 ] )
	while True:
	# Drive forward
	bot.setMotorSpeeds( speed, speed )
	time.sleep( 3.0 )
	# Turn right
	bot.setMotorSpeeds( speed, -speed )
	time.sleep( speed /(255*4.0) )

view raw drive_in_square.py hosted with ❤ by GitHub

Notice that the speed can be specified in the command line which runs the program.  Lines 11 and 12 pick up the argument if you put it in, and otherwise it assigns a default value. For example, if you enter the command:
python drive_in_square.py
the default value of speed at line 9 will be used, while the command
python Drive_in_square.py 255
will enable the command line argument with the value 255 to be picked up, and this value will override line 9.

monitor_ultrasonic.py
This program causes the ultrasonic transceiver to transmit and receive, so that the estimated distance to a solid object can be printed on the remote terminal.  The PiBot doesn't move.

Here it is running:

It may be a little difficult to see, but the screen gives a continuous readout of the distance from the ultrasound detector to a solid object - for example, my hand.

Here's the code:

	#! /usr/bin/env python
	# monitor_ultrasonic.py
	import time
	import pibot
	bot = pibot.PiBot()
	while True:
	print "Distance is", bot.getUltrasonicDistance()
	time.sleep( 0.1 )

view raw monitor_ultrasonic.py hosted with ❤ by GitHub

robot_teleop.py
The teleop program makes the PiBot respond to 1-character keyboard commands - f for forward, b for backward, l for left, r for right turn and s for stop, each followed by an Enter. 

Here it is running:

It's not very slick, but remember that these are only test programs for checking that the hardware works properly.  The glamour comes later!

Here's the code:

	#! /usr/bin/python
	# robot_teleop.py
	import pibot
	import time
	TURN_TIME = 0.2
	bot = pibot.PiBot()
	while True:
	# Read commands from the user
	command = raw_input( ": " )
	command = command.strip().lower()
	if len( command ) > 0:
	commandLetter = command[ 0 ]
	if commandLetter == "f":
	bot.setMotorSpeeds( 128, 128 )
	elif commandLetter == "b":
	bot.setMotorSpeeds( -128, -128 )
	elif commandLetter == "l":
	bot.setMotorSpeeds( -128, 128 )
	time.sleep( TURN_TIME )
	bot.setMotorSpeeds( 0, 0 )
	elif commandLetter == "r":
	bot.setMotorSpeeds( 128, -128 )
	time.sleep( TURN_TIME )
	bot.setMotorSpeeds( 0, 0 )
	elif commandLetter == "s":
	bot.setMotorSpeeds( 0, 0 )

view raw robot_teleop.py hosted with ❤ by GitHub

test_pibot_hardware.py
This program tests all hardware connected to the PiBot.  Currently the final kit parts have not been fitted - the panning step motor and the tilting servo for aiming the PiCam, but the code for some of these functions is already there, ready for connection.

Here it is running:

The sound at the start is of bongos, followed by a 'drum-roll', and at the end of the routine there are 4 sounds - a klaxon sound, followed by a 'ding', followed by 2 cat meeows.  The distance estimated by the ultrasound transducer is displayed on the computer terminal, as in the previous program, monitor_ultrasonic.py.  The hardware that is attached appears to be working properly, including the 8-neopixel strip.   

The stepper motor and servo software is included, but this hardware hasn't been attached yet.

Here's the code:

	#! /usr/bin/env python
	# test_pibot_hardware.py
	import time
	import os.path
	import sys
	import pygame
	import pygame.mixer
	import picamera
	import pibot
	MUSIC_VOLUME = 0.5
	TEST_TIME = 10.0
	scriptPath = os.path.dirname( __file__ )
	# Load test result sounds
	pygame.init()
	pygame.mixer.init()
	time.sleep(1)
	pygame.mixer.music.load( scriptPath + "musical073.mp3" )
	pygame.mixer.music.set_volume( MUSIC_VOLUME * 16)
	pygame.mixer.music.play()
	time.sleep(7)
	pygame.mixer.music.load( scriptPath + "drum_roll.mp3" )
	pygame.mixer.music.set_volume( MUSIC_VOLUME * 16)
	pygame.mixer.music.play()
	time.sleep(5)
	camera.Picamera()
	try:
	camera.start_preview()
	time.sleep(10)
	camera.stop_preview()
	finally:
	camera.close()
	# Connect to the PiBot
	bot = pibot.PiBot()
	testStartTime = time.time()
	while time.time() - testStartTime < TEST_TIME:
	curTime = time.time() - testStartTime
	# Set motor speeds
	if int( curTime )%2 == 0:
	bot.setMotorSpeeds( 192, -192 )
	bot.setStepperSpeed( 255 )
	for pixelIdx in range( bot.NUM_NEO_PIXELS ):
	bot.setNeoPixelColour( pixelIdx, 128, 0, 0 )
	else:
	bot.setMotorSpeeds( -192, 192 )
	bot.setStepperSpeed( -255 )
	for pixelIdx in range( bot.NUM_NEO_PIXELS ):
	bot.setNeoPixelColour( pixelIdx, 0, 0, 128 )
	# Set servo angle
	bot.setServoAngle( (curTime / TEST_TIME) * 180.0 )
	print "Ultrasonic distance =", bot.getUltrasonicDistance()
	time.sleep( 0.05 )
	for pixelIdx in range( bot.NUM_NEO_PIXELS ):
	bot.setNeoPixelColour( pixelIdx, 0, 128, 0 )
	del bot
	time.sleep(1)
	pygame.mixer.music.load( scriptPath + "failure.mp3" )
	pygame.mixer.music.set_volume( MUSIC_VOLUME * 4 )
	pygame.mixer.music.play()
	time.sleep(3)
	pygame.mixer.music.load( scriptPath + "success.mp3" )
	pygame.mixer.music.set_volume( MUSIC_VOLUME )
	pygame.mixer.music.play()
	time.sleep(1)
	pygame.mixer.music.load( scriptPath + "animals023.mp3" )
	pygame.mixer.music.set_volume( MUSIC_VOLUME * 16 )
	pygame.mixer.music.play()
	time.sleep(3)
	pygame.mixer.music.load( scriptPath + "animals023.mp3" )
	pygame.mixer.music.set_volume( MUSIC_VOLUME * 16 )
	pygame.mixer.music.play()
	time.sleep(3)
	while pygame.mixer.music.get_busy():
	pass

view raw KCtest2.py hosted with ❤ by GitHub

	#! /usr/bin/env python
	# test_pibot_hardware.py
	import time
	import os.path
	import sys
	import pygame
	import pygame.mixer
	import pibot
	MUSIC_VOLUME = 0.5
	TEST_TIME = 10.0
	scriptPath = os.path.dirname( __file__ )
	# Load test result sounds
	pygame.init()
	pygame.mixer.init()
	time.sleep(1)
	pygame.mixer.music.load( scriptPath + "musical073.mp3" )
	pygame.mixer.music.set_volume( MUSIC_VOLUME * 16)
	pygame.mixer.music.play()
	time.sleep(7)
	pygame.mixer.music.load( scriptPath + "drum_roll.mp3" )
	pygame.mixer.music.set_volume( MUSIC_VOLUME * 16)
	pygame.mixer.music.play()
	time.sleep(5)
	# Connect to the PiBot
	bot = pibot.PiBot()
	testStartTime = time.time()
	while time.time() - testStartTime < TEST_TIME:
	curTime = time.time() - testStartTime
	# Set motor speeds
	if int( curTime )%2 == 0:
	bot.setMotorSpeeds( 192, -192 )
	bot.setStepperSpeed( 255 )
	for pixelIdx in range( bot.NUM_NEO_PIXELS ):
	bot.setNeoPixelColour( pixelIdx, 128, 0, 0 )
	else:
	bot.setMotorSpeeds( -192, 192 )
	bot.setStepperSpeed( -255 )
	for pixelIdx in range( bot.NUM_NEO_PIXELS ):
	bot.setNeoPixelColour( pixelIdx, 0, 0, 128 )
	# Set servo angle
	bot.setServoAngle( (curTime / TEST_TIME) * 180.0 )
	print "Ultrasonic distance =", bot.getUltrasonicDistance()
	time.sleep( 0.05 )
	for pixelIdx in range( bot.NUM_NEO_PIXELS ):
	bot.setNeoPixelColour( pixelIdx, 0, 128, 0 )
	del bot
	time.sleep(1)
	pygame.mixer.music.load( scriptPath + "failure.mp3" )
	pygame.mixer.music.set_volume( MUSIC_VOLUME * 4 )
	pygame.mixer.music.play()
	time.sleep(3)
	pygame.mixer.music.load( scriptPath + "success.mp3" )
	pygame.mixer.music.set_volume( MUSIC_VOLUME )
	pygame.mixer.music.play()
	time.sleep(1)
	pygame.mixer.music.load( scriptPath + "animals023.mp3" )
	pygame.mixer.music.set_volume( MUSIC_VOLUME * 16 )
	pygame.mixer.music.play()
	time.sleep(3)
	pygame.mixer.music.load( scriptPath + "animals023.mp3" )
	pygame.mixer.music.set_volume( MUSIC_VOLUME * 16 )
	pygame.mixer.music.play()
	time.sleep(3)
	while pygame.mixer.music.get_busy():
	pass

view raw test_pibot_hardware.py hosted with ❤ by GitHub

Notice that I am using a couple of mp3 sound bites downloaded from the internet, including one of a cat meeow, hoping to stimulate some interest in our cat, but it doesn't fool her - she just ignores it!

What's next - I hear you say - well, some tidying up of the above, and dreaming up what I could make it do etc.

Some observations so far - the 4 x AA rechargeable batteries work a treat, and for a surprisingly long time.  I have 2 sets of 4 x 2400 mA-h AAs, so one set can be charging while I'm running the PiBot with the other set.  I haven't timed the workload available, but so far I have only had one episode (the Pi wouldn't boot) where the only remedy seemed to be to replace the batteries, and that worked.

Presumably by the time I start running the PiCam, the system will start to eat up batteries.  I'm happy that the Pi I'm using is a Model A because the power consumption is less (1.5W) than that of the Model B (3.5W).

Things I would like to do?

1. I assume that I will be able to use the ultrasound distance system to prevent the PiBot from colliding with things - my SD card is getting an awful battering!
2. Experimenting with combinations of colours of the neopixels.
3. Of course, mounting the camera and transmitting images would be great. (ref HERE).
4. I also have a successful XBee pair system working away (ref HERE), so I will try to think how that could be included in the PiBot.
5. An IR receiver would be great, to smarten up the remote control. (ref HERE).
6. I saw some code for a thermistor, and I would like to incorporate my previously-built thermopile system (ref HERE), which is still working away on my desk, telling me how cold a bag of ice is, or how hot my tea is.
7. Another project which is working away on my desk very successfully, is the recently built light follower (ref HERE) - it works a treat and I would like to somehow incorporate that into the PiBot.
8. I would like to put a powerful headlamp system on.  This could be self-powering like a high-power LED flashlight (or two?) (ref HERE).
9. A talking system would be good.
10. Get it to make my tea, wash the car.........

But first, I will try to fully understand and maybe explain, the current software.