Ball Balancing – V2


Same story here as with the last couple of posts: these are some rather old attempts at improving a previous project, without much success, even after a considerable amount of effort… Hence the decision to still create a post as a future reference.

Remember the goal of this project was to ultimately be able to solve the maze automatically.

The 1st step for that, was to be able to precisely control the position of the ball, which in itself has 2 challenges: detect the position of the ball (which has to be reliable and fast) and then be able to control the plate using PID algorithms to move the ball to the desired position.

With the above done, I could then recognise the shape of the labyrinth (which I actually managed to do from the beginning, but never got to use it) and move the ball in small increments from one intermediary position to the next.

Going back to precisely controlling the position, the results presented in the initial post, were rather encouraging but the error was too big, ~1-2cm.

There were several things I thought I could improve:

Make the detection in OpenCV faster

For this, I simply bought a Raspberry Pi 2.

I’ve also tried to use a normal camera but went back to the IR one as it gave more reliable results.

I’ve also spent quite a while playing with various algorithms in OpenCV, like HoughCircles(), dilate(), erode(), using the HSV colour space and then inRange(), Gaussian blur(), adaptiveThreshold() and all their combinations.
None of which I managed to make work better or faster then the quite “naive” approach I’ve settled on: using a grey image, I do threshold(), dilate(), findContours() and then minEnclosingCircle().
I then filter by the size of the generated circles and take the one that is closest to the last detected one (this nicely gets rid of rare but nasty random detections at the other end of the platter).

In the end, even painting the platter in black didn’t seem to help much…

Make the platter movement more precise

Whereas initially I had simply bolted the RC Servos onto the original mechanism:

DSC07310

Initial Platter Mechanism

Now I’m using a few Lego bits to make a tighter connection:

DSC07311

Lego pieces based connection

 

20150916_002714

Testing the new, Lego based connections

 


 

Eliminate jitter from the Servos

I found out that a certain amount of jitter was coming from the fact that the Arduino can’t reliably generate the PWM timings.

After much back and forth I decided to completely replace the Arduino with a Stellaris LaunchPad, which has an ARM 32bit processor running at 80MHz and hence won’t have issues generating the 2 signals and doing high speed SPI at the same time.

20160328_211943

The Stellaris LaunchPad, connected to the 2 servos and the Raspberry Pi (SPI)

 

And here’s the code, after I’ve spend quite a lot of time migrating all this functionality, but it’s not wasted as it allowed me to (re)discover the Launchpad and be quite pleasantly surprised by it.
The below is also one of the main reasons I’m writing this post, I really wanted to paste this example somewhere, as it contains a few useful bits, like controlling the timers, the serial port, SPI and of course a more or less generic PID implementation, in its own class.

main.cpp

#include "inc/hw_types.h"
#include "inc/hw_memmap.h"

#include "driverlib/interrupt.h" //include hw_types.h before including interrupt.h
#include "driverlib/sysctl.h"
#include "driverlib/gpio.h"
#include "driverlib/timer.h"
#include "driverlib/ssi.h"
#include "driverlib/uart.h"
#include "driverlib/systick.h"
#include "driverlib/interrupt.h"

#include "utils/uartstdio.h"

#include "PID.h"

////////////////// Microseconds //////////////////////////////////

static unsigned long _milliseconds = 0;
unsigned long millis() {
	return _milliseconds;
}

void sysTickIntHandler() {
	_milliseconds++;
}

void initMicroseconds() {
	SysTickPeriodSet(SysCtlClockGet() / 1000);
	SysTickEnable();
	SysTickIntRegister(sysTickIntHandler);
	SysTickIntEnable();
}

////////////////////////////////////////////////////

////////////////// Servo ~ PWM //////////////////////////////////

unsigned long _clockMicros;

void initServoTimers() {
	_clockMicros = SysCtlClockGet() / 1000000;

	// Wide Timer 0 -> WT0CCP0 / PC4 & WT0CCP1 / PC5  (Table 11-2)
	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
	GPIOPinTypeGPIOOutput(GPIO_PORTC_BASE, GPIO_PIN_4 | GPIO_PIN_5);
	GPIOPinWrite(GPIO_PORTC_BASE, GPIO_PIN_4 | GPIO_PIN_5, 0x00);

	// Pin Mux-ing
	GPIOPinConfigure(GPIO_PC4_WT0CCP0);
	GPIOPinConfigure(GPIO_PC5_WT0CCP1);
	// give control of the pins to the Timer hardware
	GPIOPinTypeTimer(GPIO_PORTC_BASE, GPIO_PIN_4 | GPIO_PIN_5);

	SysCtlPeripheralEnable(SYSCTL_PERIPH_WTIMER0);
	// split the 64bit timer into 2x 32 bits
	TimerConfigure(WTIMER0_BASE, TIMER_CFG_SPLIT_PAIR | TIMER_CFG_A_PWM | TIMER_CFG_B_PWM);
	// invert the PWM
	TimerControlLevel(WTIMER0_BASE, TIMER_A, true);
	TimerControlLevel(WTIMER0_BASE, TIMER_B, true);

	// The Servo period is normally 20ms, but we use 15ms here to make it more reactive ?
	unsigned long SERVO_PERIOD_MICROS = 15000;
	TimerLoadSet(WTIMER0_BASE, TIMER_A, SERVO_PERIOD_MICROS * _clockMicros);
	// the pulse has to be between 500 and 2500 micros
	TimerMatchSet(WTIMER0_BASE, TIMER_A, 1500 * _clockMicros);
	TimerEnable(WTIMER0_BASE, TIMER_A);

	TimerLoadSet(WTIMER0_BASE, TIMER_B, SERVO_PERIOD_MICROS * _clockMicros);
	// the pulse has to be between 500 and 2500 micros
	TimerMatchSet(WTIMER0_BASE, TIMER_B, 1500 * _clockMicros);
	TimerEnable(WTIMER0_BASE, TIMER_B);
}

unsigned int constrainPulse(unsigned int micros) {
	if (micros < 600) return 600; 	else if (micros > 2400) return 2400;
	else return micros;
}

// WT0CCP0 / PC4
void servoAMicros(unsigned int micros) {
	TimerMatchSet(WTIMER0_BASE, TIMER_A, constrainPulse(micros) * _clockMicros);
}

// WT0CCP1 / PC5
void servoBMicros(unsigned int micros) {
	TimerMatchSet(WTIMER0_BASE, TIMER_B, constrainPulse(micros) * _clockMicros);
}

////////////////////////////////////////////////////

///////////// SPI3 ///////////////////////////////////////

unsigned char END_MSG = 254;
unsigned long _spiData[30];
unsigned char _spiDataIdx = 0;

unsigned char _x, _y, _setPointX, _setPointY;
unsigned char _process = 0;

void SPI3IntHandler() {
	// Keeps the interrupts from being triggered again immediately upon exit
	SSIIntClear(SSI3_BASE, SSI_RXFF);

	// this is a very very basic custom protocol, that marks the end of a valid message and does some error checking
	unsigned long received;
	while(SSIDataGetNonBlocking(SSI3_BASE, &received)){
		_spiData[_spiDataIdx ++] = received;

		if(received == END_MSG) {
			if(_spiDataIdx == 6) {
				unsigned char receivedChecksum = _spiData[_spiDataIdx - 2];
				unsigned char checksum = 0;
				unsigned char i = 0;
				for(i = 0; i < _spiDataIdx - 2; i++){
					checksum += _spiData[i];
				}

				// the checksum has to be a byte, but also differet from END_MSG
				if(checksum == END_MSG) {
					checksum = END_MSG - 1;
				}

				// only "PUBLISH" the data if it's correct
				if(checksum == receivedChecksum) {
					_x = _spiData[0];
					_y = _spiData[1];
					_setPointX = _spiData[2];
					_setPointY = _spiData[3];
					_process = 1;
				}

				// send back the checksum
				SSIDataPut(SSI3_BASE, checksum);
			} else {
				// send back wrong checksum as the format if wrong...
				SSIDataPut(SSI3_BASE, END_MSG - 3);
			}

			// dealt with the END_MSG
			_spiDataIdx = 0;
		}
	}
}

void initSPI3() {
	// enable SSI3 peripheral and port D where the pins are
	SysCtlPeripheralEnable(SYSCTL_PERIPH_SSI3);
	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);

	// Pin Mux-ing
	GPIOPinConfigure(GPIO_PD0_SSI3CLK);
	GPIOPinConfigure(GPIO_PD1_SSI3FSS);
	GPIOPinConfigure(GPIO_PD2_SSI3RX);
	GPIOPinConfigure(GPIO_PD3_SSI3TX);
	// Give control of the pins to the SSI hardware
	GPIOPinTypeSSI(GPIO_PORTD_BASE, GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2 | GPIO_PIN_3);

	// Configure SSI in SLAVE mode
	SSIConfigSetExpClk(SSI3_BASE, SysCtlClockGet(), SSI_FRF_MOTO_MODE_3, SSI_MODE_SLAVE, 4000000, 8);
	// enable SSI specific interrupts and define the handler
	SSIIntRegister(SSI3_BASE, SPI3IntHandler);
	// Disable everything to start clean.
	SSIIntDisable(SSI3_BASE, SSI_TXFF | SSI_RXFF | SSI_RXTO | SSI_RXOR);
	// RX timeout - get the data asap, even if the FIFO is less than half full
	// RX FIFO half full or more - if we only have the timeout, if the master sends more than the size of the FIFO at once, it will overflow
	SSIIntEnable(SSI3_BASE, SSI_RXTO | SSI_RXFF);

	// Enable
	SSIEnable(SSI3_BASE);
}

////////////////////////////////////////////////////

///////////// Console ~ UART0 ///////////////////////////////////////

// UART0 is mapped by the Stellaris Launchpad to the Virtual COM port available through Debug USB
void initConsole() {
	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
	GPIOPinConfigure(GPIO_PA0_U0RX);
	GPIOPinConfigure(GPIO_PA1_U0TX);
	GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);

	SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
	// Use precision internal oscillator which runs at 16MHz
	UARTClockSourceSet(UART0_BASE, UART_CLOCK_PIOSC);
	// wow... never used UART at almost 1MHz before πŸ™‚
	UARTStdioConfig(0, 921600, 16000000);

	UARTprintf("\nUART Console initialised.\n");
}

////////////////////////////////////////////////////

#define SERVO_MAX_DELTA 200
#define SERVO_MID_POS 1500

int main() {
	// use 80MHz
	SysCtlClockSet(SYSCTL_SYSDIV_2_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);

	// Does not affect the set of interrupts enabled in the interrupt controller
	// It just gates the single interrupt from the controller to the processor
	IntMasterEnable();

	initMicroseconds();
	initConsole();
	initServoTimers();
	initSPI3();

	PID pidA(0.7, 0, 3.5, -SERVO_MAX_DELTA, SERVO_MAX_DELTA, millis);
	pidA.updateSetPoint(100);
	PID pidB(0.8, 0, 4, -SERVO_MAX_DELTA, SERVO_MAX_DELTA, millis);
	pidB.updateSetPoint(100);

	while(1) {
		if(_process) {
			_process = 0;

			pidA.updateSetPoint(_setPointX);
			int pidValueX = (int)pidA.compute(_x);
			servoAMicros(SERVO_MID_POS - pidValueX);

			pidB.updateSetPoint(_setPointY);
			int pidValueY = (int)pidB.compute(_y);
			servoBMicros(SERVO_MID_POS - pidValueY);

			UARTprintf("X: %d ~ %d / Y: %d ~ %d\n", _x, pidValueX, _y, pidValueY);
		}
	}
}

//void initRGBLEDs() {
//	// RGB LEDs
//	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
//	GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_1 | GPIO_PIN_2 | GPIO_PIN_3);
//	GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_1 | GPIO_PIN_2 | GPIO_PIN_3, 0x00);
//
//	GPIOPinConfigure(GPIO_PF1_T0CCP1); // Red LED
//	GPIOPinConfigure(GPIO_PF2_T1CCP0); // Blue LED
//	GPIOPinConfigure(GPIO_PF3_T1CCP1); // Green LED
//	GPIOPinTypeTimer(GPIO_PORTF_BASE, GPIO_PIN_1 | GPIO_PIN_2 | GPIO_PIN_3 );
//
//	SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);
//	SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER1);
//	TimerConfigure(TIMER0_BASE, TIMER_CFG_SPLIT_PAIR | TIMER_CFG_B_PWM);
//	TimerConfigure(TIMER1_BASE, TIMER_CFG_SPLIT_PAIR | TIMER_CFG_A_PWM | TIMER_CFG_B_PWM);
//
//	TimerLoadSet(TIMER0_BASE, TIMER_B, 100);
//	TimerMatchSet(TIMER0_BASE, TIMER_B, 99);
//	TimerEnable(TIMER0_BASE, TIMER_B);
//
//	TimerLoadSet(TIMER1_BASE, TIMER_A, 100);
//	TimerMatchSet(TIMER1_BASE, TIMER_A, 99);
//	TimerEnable(TIMER1_BASE, TIMER_A);
//
//	TimerLoadSet(TIMER1_BASE, TIMER_B, 100);
//	TimerMatchSet(TIMER1_BASE, TIMER_B, 99);
//	TimerEnable(TIMER1_BASE, TIMER_B);
//}

PID.h

Simple utility class, I wanted to make this generic so that it can be re-used.

#ifndef PID_H_
#define PID_H_

class PID {
public:
	PID(double _kP, double _kI, double _kD, double _outMin, double _outMax, unsigned long (*_millis)(void)):
		kP(_kP), kI(_kI), kD(_kD), outMin(_outMin), outMax(_outMax), millis(_millis)
	{
		lastTime = millis();
	}

	void updateSetPoint(double _setPoint) {
		setPoint = _setPoint;
	}

	double compute(double input) {
		unsigned long now = millis();
		double error = setPoint - input;
		iTerm += (kI * error);
		iTerm = constrain(iTerm, outMin / 4, outMax / 4);

		double output = kP * error + iTerm + kD * (lastInput - input);

		// for next time
		lastInput = input;
		lastTime = now;

		return constrain(output, outMin, outMax);
	}

private:
	double kP, kI, kD, setPoint, lastInput, outMin, outMax, iTerm;
	unsigned long lastTime;
	// function to use to get the current system milliseconds
	unsigned long (*millis)(void);

	double constrain(double value, double min, double max) {
		if(value < min) return min; 		if(value > max) return max;
		return value;
	}
};

#endif /* PID_H_ */

Finally, in spite of all the above “improvements” the performance is no better than in the original version !

20160328_211741

Overall aspect of version 2

Advertisements

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s

%d bloggers like this: