New project


For last few days I was working on a new project: a self-contained 4WD table-top platform. Using an ARM CortexM0 (LPC1225) to control all peripherals: 4 motors (8 PWM, 4 encoders, over-current monitoring, PID, etc), 2 UART (one shared with an XBEE socket for wifi communication including firmware upgrade over-the-air), I2C (EEPROM, gyro+Accelerometer with MPU6050, external compass, etc), SPI port, 4 bumpers, LEDs…

So far I got the PWM working fine for all 4 motors, the I2C communication with MPU6050 and EEPROM, UART communication and all the GPIOs.

Geting the gyro and accelerometer to work was quite a lot of fun and easier than expected. Anyway, was the first time when I really played with this and to understand more on how it works I wanted to visualise it so I managed to get some code together in processing to show me real-time the position in space:

4wd and processing

 

A real-time 3D model shown using PROCESSING (gyro X and Y not shown since not used)

 

 

This will show me a “3D” representation of my platform. For this I am using only data from all 3 axis on accelerometer and Z axis from the gyro. Ok, I cheated, instead of doing all the complex calculations for YAW, I did it simply by calculating the response from gyro in degrees first:

	mpuGetRotationZ(&gZ);

	gyrZ = gZ - gyrOffsetZ;
	gyrRateZ = (float)gyrZ/gyrSens;

Where the sensitivity used is one from the datasheet for the selected range (e.g.: in 131 degrees for the 250 degrees per second range). Once this calculated, I have a thread triggered every 25ms to update the position by summing up the delta returned by the gyro then send out the result:

	mpuPosition();
	yawPos += gyrRateZ;

For accelerometer I used a different approach: just push the data straight to PROCESSING and do the calculations there using following formulas:

void Get_Accel_Angles(){
  xAngl = atan((float)acc[1]/ sqrt(pow((float)acc[2],2)+pow((float)acc[0],2)));
  yAngl = atan((float)-acc[0]/ sqrt(pow((float)acc[2],2)+pow((float)acc[1],2)));  
}

Passing this results to PROCESSING over a serial line then use it to rotate the 3D shape previously defined:

void drawTarget(){
  pushMatrix();
    translate(VIEW_SIZE_X/2, VIEW_SIZE_Y/2+50, -50);
    scale(4,4,4);

    rotateZ(xAngl);
    rotateX(yAngl);
    rotateY(gyr[z]);

    buildShape();
  popMatrix(); 
}

The results are pretty accurate unless either rotate it extremely slow or too fast (which can be fixed by changing the range to a higher sensitivity), Adding a compass in the mix will solve this too however.

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