Blutack

In dedication to one of the most useful things in my room…Blutack

Oh blutack, why art though so awesome? You gotta love these things, they help people like me who’s lousy in fabrication. This post is dedicated to you, since you helped create it.

Pan and tilt system using two servos and the yaw and pitch axes

That’s right! This is just an upgrade to my last video. This time I’m using two servos and utilising two axes(axii?). As I’ve said above this video is dedicated to the almighty blutack, and you’ll understand what I mean when you see the photos below. I was meaning to post earlier but just got off work a couple of hours ago, I just wanted to do something rough before I hit the hay as the goddess of luck and robotics smiled upon me when I asked Ted if he has another servo. I’ve been itching to do this for a while now, and the itch has been cured…for now! Again this is very rough work, this is probably the first time you’ll ever see such a thing as a cardboard bracket with blutack hinges. But I plan to upgrade with sugru soon, if I manage to get my hands with some more of them. Anyway without further ado, Blue the blutack jointed robot(ps please watch videos in 720p or 1080p as it looks horrible on 480p):

20121107-003723.jpg

20121107-003752.jpg


//
// This program interfaces a Wii wii motion plus with the dsPIC using the I2C module
// Pan and tilt using two servos
// Program written by Gian Lorenzo
// Last edited 07/11/2012 1:39AM
//

#include <libpic30.h>
#include <p30f4011.h>
#include <stdio.h>
#include <xc.h>
#include <i2c.h>
#include <math.h>

// Configuration settings
_FOSC(CSW_FSCM_OFF & FRC_PLL16);  // Fosc=16x7.5MHz, Fcy=30MHz
_FWDT(WDT_OFF);                   // Watchdog timer off
_FBORPOR(MCLR_DIS);               // Disable reset pin

// Function prototypes
void setup();								// Configure pins etc
unsigned int read_analog_channel(int n);	// Reading analog channel
void wiimp_init();						// wiimp initialisation/Handshake
void calibrate();
void wiimp_read(unsigned char*);			// Reading data from the Wii wiimp              // Nunchuk data array
void pause(unsigned int ms);				// Pause function(in ms)
void forward();								// Moves the Servo forward
void back();								// Moves the Servo backward

double yawzero=8054.7,pitchzero=7744,rollzero=7745,
yaw,roll,pitch, deltayaw, deltaroll,deltapitch;
unsigned char  wii_data[20],c; 


int main()
{
	unsigned int ax,ay,az;  // Data containers
	setup();				// Set up which pins are which and set up i2c and configs
	pause(1000);			// 1 second startup delay
	wiimp_init();			// Initialise wii motio plus
	pause(1000);
	//calibrate();			// Calibrate for 0
	pause(1000);
	int o = 0;
	
	while(1)
	{
		if(o == 0)	// Get rid of bad readings
		{
			wiimp_read(wii_data);                      
			wiimp_read(wii_data);
			wiimp_read(wii_data);
			wiimp_read(wii_data);
			wiimp_read(wii_data);
			wiimp_read(wii_data);
			wiimp_read(wii_data);
			pause(2000);
			o++;
		}
				
		wiimp_read(wii_data);                       // Read data from nunchuk

		ax = ((wii_data[3] & 0xFC) << 6) + wii_data[0];	// Extract gyroscope yaw byte
		ay = ((wii_data[4] & 0xFC) << 6) + wii_data[1];	// Extract gyroscope roll byte
		az = ((wii_data[5] & 0xFC) << 6) + wii_data[2];	// Extract gyroscope pitch byte
		
		yaw = ax - yawzero;
		roll = ay - rollzero;
		pitch = az - pitchzero;
		
		deltayaw = deltayaw + yaw;
		deltaroll = deltaroll + roll;
		deltapitch = deltapitch + pitch;
		
		
		if (U2STAbits.URXDA == 1)			//recalibration using uart
		{
			c = U2RXREG;
			if (c == 'y')	deltayaw = 0;
			else if(c == 'p')	deltapitch =0;
			else if(c =='r')	deltaroll =0;
			else if(c == 'a')	deltayaw =0;deltapitch=0;deltaroll=0;
		}
		PDC2 = 1410+(deltapitch*0.06083333333333/2);
		PDC1 = 1280-(deltayaw*.06083333333333/2);
		pause(20);
		//printf("%1d\r %1d\r %1d\r\r\n", ax, ay, az);
		printf("%1f\r %1f\r %1f\r\r\n", deltayaw, deltaroll, deltapitch);
	}

    return 0;
}
void forward()
{
	if(PDC1 < 2010)
	{
		PDC1 = PDC1 + 20;
		pause(.5);
	}
}
void back()
{
	if(PDC1 > 550)
	{
		PDC1 = PDC1 - 20;
		pause(.5);
	}
}

void setup()
{
    // Setup UART
	__C30_UART=2;			// Use secondary UART
    U2BRG = 48;            	// 38400 baud @ 30 MIPS
    U2MODEbits.UARTEN = 1; 	// Enable UART

    // Configure D ports as a digital outputs
    LATD = 0;
    TRISD = 0b11110000;

    // Configure analog inputs
    TRISB = 0x01FF;      /* Port B all inputs */
    ADPCFG = 0xFF00;     /* PORTB 0-7 are analog inputs */
    ADCON1 = 0;          /* Manually clear SAMP to end sampling, start conversion*/
    ADCON2 = 0;          /* Voltage reference from AVDD and AVSS */
    ADCON3 = 0x0005;     /* Manual Sample, ADCS=5 -> Tad = 3*Tcy = 0.1us */
    ADCON1bits.ADON = 1; /* Turn ADC ON */

    // Configure PWM
    // PWM period = Tcy * prescale * PTPER = 0.33ns * 64 * 9470 = 20ms
    PWMCON1 = 0x00FF; // Enable all PWM pairs in complementary mode
    PTCON = 0;
    _PTCKPS = 3;      // prescale=1:64 (0=1:1, 1=1:4, 2=1:16, 3=1:64)
    PTPER = 9470;     // 20ms PWM period (15-bit period value)
    PDC1 = 0;         // 0% duty cycle on channel 1 (max is 65536)
    PDC2 = 0;         // 0% duty cycle on channel 2 (max is 65536)
    PDC3 = 0;         // 0% duty cycle on channel 3 (max is 65536)
    PTMR = 0;         // Clear 15-bit PWM timer counter
    _PTEN = 1;        // Enable PWM time base
	
	// Configure I2c
	I2CCONbits.I2CSIDL = 1;	// Stop module in idle mode
	I2CCONbits.A10M = 0;	// 7-bit address
	I2CCONbits.DISSLW = 0;	// slew rate control. Required to be on for 400kHZ
	I2CBRG = 272;			// 272 for 100kHz at FCY = 30MHz formula next line
							// I2CBRG = (((1/rate)-900ns)*FCY)-1
	I2CCONbits.I2CEN = 1;	// Enable i2c
}

void calibrate()
{
	int i=0,o=0;
    for (o=0;o<10;o++)
      {
        wiimp_read(wii_data);
		pause(10);
      }
	wiimp_read(wii_data);
    yawzero=((wii_data[3]>>6)+wii_data[0]);
    rollzero=((wii_data[4]>>6)+wii_data[1]);
    pitchzero=((wii_data[5]>>6)+wii_data[2]);
    for (i=0;i<10;i++)
      {
        wiimp_read(wii_data);
        yawzero=((wii_data[3]>>6)+wii_data[0])+yawzero/2; //average 10 readings
        rollzero=((wii_data[4]>>6)+wii_data[1])+rollzero/2;
        pitchzero=((wii_data[5]>>6)+wii_data[2])+rollzero/2;
      }
	printf("%f %f %f", yawzero,rollzero,pitchzero);
	pause(2000);
}
void wiimp_init(void) 
{
	StartI2C();			// Open I2C bus
	IdleI2C();			// Wait for last command to finish
	MasterWriteI2C(0xA6);// Call device adress 53(Wii wiimp)
	MasterWriteI2C(0xFE);// Handshake protocol for wii wiimp	
	MasterWriteI2C(0x04);
	StopI2C();
	printf("connected\r\n");
}

void wiimp_read(unsigned char* array) 
{
	int idx;	// Counter

	// ---- Request measurement ---- //
	StartI2C();		// Open I2C bus
	IdleI2C();		// Wait for last command to finish
	MasterWriteI2C(0xA4); // Call the adress you want to read from
	MasterWriteI2C(0x00);	
	StopI2C();		// Stop I2C
	pause(10); 	// 3 ms or more required or acquisition goes wrong

	 //Request reading
  	StartI2C();	// Open I2C bus
	IdleI2C();	// Wait for last command to finish
	MasterWriteI2C(0xA5);	// 0XA5 is the read adress of the wii wiimp(52+1)
	IdleI2C();	// Wait for last command to finish
	 //Bytes reception
	for(idx = 0; idx < 5; idx++)
	{
		array[idx] = MasterReadI2C(); 	// Store the values into an array
		AckI2C(); 						// Send an ACK to notify wiimp to send next byte
		IdleI2C();						// Wait for last command to finish
	}
	pause(1);
	array[5] = MasterReadI2C(); 		// Last byte to be read
	NotAckI2C(); IdleI2C();				// Send a NACK to tell wiimp that reading is done
	StopI2C();							// Stop I2C
}

// Analogue input reading
unsigned int read_analog_channel(int channel)
{
    ADCHS = channel;          // Select the requested channel
    ADCON1bits.SAMP = 1;      // start sampling
    __delay32(30);            // 1us delay @ 30 MIPS
    ADCON1bits.SAMP = 0;      // start Converting
    while (!ADCON1bits.DONE); // Should take 12 * Tad = 1.2us
    return ADCBUF0;
}
void pause(unsigned int ms)
{
	__delay32(30000L*ms);	// 1ms Pause = (ms * FCY/1000)
}
Advertisements

Interfacing a Wii motion plus to the dspic30F4011

I’m at it again!

And so after my ventures with I2C and the Wii nunchuck; many prospects of the “mini project” have opened up. While the Wii nunchuck’s accelerometer is great, there’s no harm in trying out new sensors. So far I can think of two applications but they will require a lot of time and work(and money!) to build and so I’ll think about it more. The first one is sort of a crane attached onto a “roof”, Two stepper motors control the x and y coordinates of the plane and the Wii motion plus can control the movement of the actual gripper. The second one is a robot car with a crane on in and works pretty much the same as the one I said there but with wheels and is on the ground. I’m also intrigued into building something aerial but I don’t want to get too ahead of myself.

And so I got my Wii motion plus that’s lying around and got to work into trying to communicate with it. The way it communicates is also through I2C and the code I came up with is pretty much the same as with the nunchuck, only this time around I’m only reading three values as the Wii motion plus is basically a gyroscope.  I’ve got to admit I’m liking this sensor more than the nunchuck, so far anyway. I really wish I had another servo right now, as I can only utilise one axis with one servo. It’s good enough for the demo I suppose.

Gyroscope

So, what makes the wii motion plus sensor work? That could be answered in one word: A gyroscope(ok that’s two ha ha Gian can’t count). But to explain how a gyroscope works can be a bit tricky and I wont even try to explain it too deeply. The image above is one of a toy gyroscope, when I first played with one as a child I was dumbfounded and no one around me could explain how it does what it does. It’s basically constructed with a spinning wheel in the middle and round metal things around it. Without doing anything to it; it does nothing, it’s just a piece of metal. But spin the wheel and the middle and the magic happens, when the wheel is spun it seems to defy gravity as it can support itself sideways and in awkward positions and not lose its orientation. The reason behind this is because the wheel in the middle has angular velocity and will try to repel any force being applied to it. I think this video will explain in better more than I can:

So now you know what a gyroscope does. This little sensor of mine has one inside it and so it will tell me the orientation of my device in 3 axes which are the yaw angle, the roll angle, and the pitch angle. When the orientation is changed: the gyroscope will repel it and this degree of repulsion is sent down as a signal. This can be very useful indeed, so useful that every flying machine nowadays uses one to know its orientation.

Only downside is: it doesn’t know it’s initial orientation. This is the reason why everytime you want to use your wii motion plus on the console you have to put it down a flat surface before you can use it. But A-ha! We have the wii nunchuck which has an accelerometer and will tell us the current pitch of the device and so it’s a great match!

Controlling a servo using the “yaw” angle

As I’ve said I only have one servo at the moment sadly and so I had to choose one variable to use as my “controller”. I used the yaw angle randomly, this is basically the angle that goes “side-to-side” and I just scaled it to the range of the servo. That’s all that there is! Simple as. Now to talk about the I2C communication, and as I’ve said: it’s nearly the same as the wii nunchuck one, only that this sensor has a different I2C address(0x53) when trying to initialise and then it becomes 0x52 or A4 once initialised. This will cause a bit of trouble when both the Wii motion sensor and the nunchuck are being used. But there is a way of deactivating the wii motion plus with code and look at the nunchuck data, but that’s for another post. Six bytes are read from the Wii motion plus and they are as follows:

It can be seen that there are actually two modes in which the sensor operates. One is the slow mode and the other is the fast mode, this is basically when you try and change the orientation quickly or slowly, they will both have different multipliers(4 and 20) and so it has to be taken into account. And another thing is that the data is actually moved 6 bits in and so we have to read from “wii_data[x]>>6” for example. Plenty of useful stuff here if anyone is interested. And without further ado, here’s my short demo:


//
// This program interfaces a Wii wii motion plus with the dsPIC using the I2C module
//
// Program written by Gian Lorenzo
// Last edited 05/11/2012 10:26AM
//

#include <libpic30.h>
#include <p30f4011.h>
#include <stdio.h>
#include <xc.h>
#include <i2c.h>
#include <math.h>

// Configuration settings
_FOSC(CSW_FSCM_OFF & FRC_PLL16);  // Fosc=16x7.5MHz, Fcy=30MHz
_FWDT(WDT_OFF);                   // Watchdog timer off
_FBORPOR(MCLR_DIS);               // Disable reset pin

// Function prototypes
void setup();								// Configure pins etc
unsigned int read_analog_channel(int n);	// Reading analog channel
void wiimp_init();						// wiimp initialisation/Handshake
void calibrate();
void wiimp_read(unsigned char*);			// Reading data from the Wii wiimp              // Nunchuk data array
void pause(unsigned int ms);				// Pause function(in ms)
void forward();								// Moves the Servo forward
void back();								// Moves the Servo backward

double yawzero=8055.5,pitchzero=7612,rollzero=7739,
yaw,roll,pitch, deltayaw, deltaroll,deltapitch;
unsigned char  wii_data[20];

int main()
{
	unsigned int ax,ay,az;  // Data containers
	setup();				// Set up which pins are which and set up i2c and configs
	pause(1000);			// 1 second startup delay
	wiimp_init();			// Initialise wii motio plus
	pause(1000);
	//calibrate();			// Calibrate for 0
	pause(1000);
	int o = 0;

	while(1)
	{
		if(o == 0)	// Get rid of bad readings
		{
			wiimp_read(wii_data);
			wiimp_read(wii_data);
			wiimp_read(wii_data);
			wiimp_read(wii_data);
			wiimp_read(wii_data);
			wiimp_read(wii_data);
			wiimp_read(wii_data);
			pause(1000);
			o++;
		}

		wiimp_read(wii_data);                       // Read data from nunchuk

		ax = ((wii_data[3] & 0xFC) << 6) + wii_data[0];	// Extract gyroscope yaw byte
		ay = ((wii_data[4] & 0xFC) << 6) + wii_data[1];	// Extract gyroscope roll byte
		az = ((wii_data[5] & 0xFC) << 6) + wii_data[2];	// Extract gyroscope pitch byte

		yaw = ax - yawzero;
		roll = ay - rollzero;
		pitch = az - pitchzero;

		deltayaw = deltayaw + yaw;
		deltaroll = deltaroll + roll;
		deltapitch = deltapitch + pitch;
		// Extract gyroscope pitch byte
		PDC1 = 1280-(deltayaw*.06083333333333);
		pause(50);
		//printf("%1d\r %1d\r %1d\r\r\n", ax, ay, az);
		printf("%1f\r %1f\r %1f\r\r\n", deltayaw, deltaroll, deltapitch);
	}

    return 0;
}
void forward()
{
	if(PDC1 < 2010) 	{ 		PDC1 = PDC1 + 20; 		pause(.5); 	} } void back() { 	if(PDC1 > 550)
	{
		PDC1 = PDC1 - 20;
		pause(.5);
	}
}

void setup()
{
    // Setup UART
	__C30_UART=2;			// Use secondary UART
    U2BRG = 48;            	// 38400 baud @ 30 MIPS
    U2MODEbits.UARTEN = 1; 	// Enable UART

    // Configure D ports as a digital outputs
    LATD = 0;
    TRISD = 0b11110000;

    // Configure analog inputs
    TRISB = 0x01FF;      /* Port B all inputs */
    ADPCFG = 0xFF00;     /* PORTB 0-7 are analog inputs */
    ADCON1 = 0;          /* Manually clear SAMP to end sampling, start conversion*/
    ADCON2 = 0;          /* Voltage reference from AVDD and AVSS */
    ADCON3 = 0x0005;     /* Manual Sample, ADCS=5 -> Tad = 3*Tcy = 0.1us */
    ADCON1bits.ADON = 1; /* Turn ADC ON */

    // Configure PWM
    // PWM period = Tcy * prescale * PTPER = 0.33ns * 64 * 9470 = 20ms
    PWMCON1 = 0x00FF; // Enable all PWM pairs in complementary mode
    PTCON = 0;
    _PTCKPS = 3;      // prescale=1:64 (0=1:1, 1=1:4, 2=1:16, 3=1:64)
    PTPER = 9470;     // 20ms PWM period (15-bit period value)
    PDC1 = 0;         // 0% duty cycle on channel 1 (max is 65536)
    PDC2 = 0;         // 0% duty cycle on channel 2 (max is 65536)
    PDC3 = 0;         // 0% duty cycle on channel 3 (max is 65536)
    PTMR = 0;         // Clear 15-bit PWM timer counter
    _PTEN = 1;        // Enable PWM time base

	// Configure I2c
	I2CCONbits.I2CSIDL = 1;	// Stop module in idle mode
	I2CCONbits.A10M = 0;	// 7-bit address
	I2CCONbits.DISSLW = 0;	// slew rate control. Required to be on for 400kHZ
	I2CBRG = 272;			// 272 for 100kHz at FCY = 30MHz formula next line
							// I2CBRG = (((1/rate)-900ns)*FCY)-1
	I2CCONbits.I2CEN = 1;	// Enable i2c
}

void calibrate()
{
	int i=0,o=0;
    for (o=0;o<10;o++)       {         wiimp_read(wii_data); 		pause(10);       } 	wiimp_read(wii_data);     yawzero=((wii_data[3]>>6)+wii_data[0]);
    rollzero=((wii_data[4]>>6)+wii_data[1]);
    pitchzero=((wii_data[5]>>6)+wii_data[2]);
    for (i=0;i<10;i++)       {         wiimp_read(wii_data);         yawzero=((wii_data[3]>>6)+wii_data[0])+yawzero/2; //average 10 readings
        rollzero=((wii_data[4]>>6)+wii_data[1])+rollzero/2;
        pitchzero=((wii_data[5]>>6)+wii_data[2])+rollzero/2;
      }
	printf("%f %f %f", yawzero,rollzero,pitchzero);
	pause(2000);
}
void wiimp_init(void)
{
	StartI2C();			// Open I2C bus
	IdleI2C();			// Wait for last command to finish
	MasterWriteI2C(0xA6);// Call device adress 53(Wii wiimp)
	MasterWriteI2C(0xFE);// Handshake protocol for wii wiimp
	MasterWriteI2C(0x04);
	StopI2C();
	printf("connected\r\n");
}

void wiimp_read(unsigned char* array)
{
	int idx;	// Counter

	// ---- Request measurement ---- //
	StartI2C();		// Open I2C bus
	IdleI2C();		// Wait for last command to finish
	MasterWriteI2C(0xA4); // Call the adress you want to read from
	MasterWriteI2C(0x00);
	StopI2C();		// Stop I2C
	pause(10); 	// 3 ms or more required or acquisition goes wrong

	 //Request reading
  	StartI2C();	// Open I2C bus
	IdleI2C();	// Wait for last command to finish
	MasterWriteI2C(0xA5);	// 0XA5 is the read adress of the wii wiimp(52+1)
	IdleI2C();	// Wait for last command to finish
	 //Bytes reception
	for(idx = 0; idx < 5; idx++)
	{
		array[idx] = MasterReadI2C(); 	// Store the values into an array
		AckI2C(); 						// Send an ACK to notify wiimp to send next byte
		IdleI2C();						// Wait for last command to finish
	}
	pause(1);
	array[5] = MasterReadI2C(); 		// Last byte to be read
	NotAckI2C(); IdleI2C();				// Send a NACK to tell wiimp that reading is done
	StopI2C();							// Stop I2C
}

// Analogue input reading
unsigned int read_analog_channel(int channel)
{
    ADCHS = channel;          // Select the requested channel
    ADCON1bits.SAMP = 1;      // start sampling
    __delay32(30);            // 1us delay @ 30 MIPS
    ADCON1bits.SAMP = 0;      // start Converting
    while (!ADCON1bits.DONE); // Should take 12 * Tad = 1.2us
    return ADCBUF0;
}
void pause(unsigned int ms)
{
	__delay32(30000L*ms);	// 1ms Pause = (ms * FCY/1000)
}