Wii Motion Plus gyros on LM3S8962 (I2C on CooCox)


After the initial discovery and a brief “hello world” on my new Arm Cortex M3 board using the CooCox environment, it’s now time for something more “involved”…

And because I’m still in the middle of my quadcopter project, and refactoring it to use the gyroscopes from a wii motion + , what better test than try to read these sensors from the new board ?

LM3S8962 with the Wii Motion Plus gyroscopes connected through I2C

Here’s the code, using CooCox.

All the I2C data transfer is quite verbose and slightly low level, I should have put the calls together in some higher level functions à la Arduino, but couldn’t be bothered :)


#include "hw_memmap.h" // for the base address of the memories and peripherals.
#include "hw_types.h"
#include "gpio.h"
#include "sysctl.h"
#include "rit128x96x4.h"     // OLED functions
#include "i2c.h"

#include <OsConfig.h>                   /*!< CoOS configure header file*/
#include <CoOs.h>                   /*!< CoOS header file               */

#define WMP_ADDR 0x52 //0xA4   // Wii Motion Plus
#define WMP_ADDR_INIT 0x53 //0xA5   // Wii Motion Plus starts at this address
#define LED GPIO_PF2_LED1
#define  TASK_STK_SIZE  128

OS_STK       taskA_Stk[TASK_STK_SIZE];
volatile int _gyroX, _gyroY, _gyroZ, _gyroX0 = 0, _gyroY0 = 0, _gyroZ0 = 0;

void initBoard(void){
 /* Set the clocking to run from PLL at 50 MHz */
 SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_8MHZ);

 /* Enable peripherals */
 SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);

 /* Configure Status LED as output */
 GPIOPadConfigSet(GPIO_PORTF_BASE, LED,    GPIO_STRENGTH_2MA, GPIO_PIN_TYPE_STD);
 GPIODirModeSet  (GPIO_PORTF_BASE, LED,    GPIO_DIR_MODE_OUT);

 /* Initialize LCD */
 RIT128x96x4Init(1000000);
 RIT128x96x4Clear();

 /* Display Initial message */
 char buf[25];
 sprintf(buf, "Wii Motion Plus - I2C");
 RIT128x96x4StringDraw(buf, 0, 5, 11);

 /* Init I2C */
 SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
 SysCtlPeripheralEnable(SYSCTL_PERIPH_I2C0);
 GPIOPinTypeI2C(GPIO_PORTB_BASE, GPIO_PIN_2 | GPIO_PIN_3 );
 // Initialize Master and Slave
 I2CMasterInitExpClk(I2C_MASTER_BASE, SysCtlClockGet(), false);
}

void initWMPGyros(){
 // Specify slave address and the fact that we SEND
 I2CMasterSlaveAddrSet(I2C_MASTER_BASE, WMP_ADDR_INIT, false); //WM+ starts out deactivated at address 0x53
 //send 0x04 to address 0xFE to activate WM+
 I2CMasterDataPut(I2C0_MASTER_BASE, 0xfe);
 I2CMasterControl(I2C0_MASTER_BASE, I2C_MASTER_CMD_BURST_SEND_START);
 while(I2CMasterBusy(I2C0_MASTER_BASE));
 I2CMasterDataPut(I2C0_MASTER_BASE, 0x04);
 I2CMasterControl(I2C0_MASTER_BASE, I2C_MASTER_CMD_BURST_SEND_CONT);
 while(I2CMasterBusy(I2C0_MASTER_BASE));
 I2CMasterControl(I2C0_MASTER_BASE, I2C_MASTER_CMD_BURST_SEND_FINISH);
 // WM+ has jumped to another address, so don't wait for it...
}

void readWMPGyros() {
 unsigned char buff[6];

 // Specify slave address and the fact that we SEND
 I2CMasterSlaveAddrSet(I2C_MASTER_BASE, WMP_ADDR, false);
 // send one 0 to ask for data
 I2CMasterDataPut(I2C0_MASTER_BASE, 0x00);
 I2CMasterControl(I2C0_MASTER_BASE, I2C_MASTER_CMD_SINGLE_SEND);
 while(I2CMasterBusy(I2C0_MASTER_BASE));

 // Specify slave address and the fact that we RECEIVE
 I2CMasterSlaveAddrSet(I2C_MASTER_BASE, WMP_ADDR, true);
 // receive the data back
 I2CMasterControl(I2C0_MASTER_BASE, I2C_MASTER_CMD_BURST_RECEIVE_START);
 while(I2CMasterBusy(I2C0_MASTER_BASE));
 buff[0] = I2CMasterDataGet(I2C_MASTER_BASE);

 int i;
 for(i=1; i<6; i++) {
 I2CMasterControl(I2C0_MASTER_BASE, I2C_MASTER_CMD_BURST_RECEIVE_CONT);
 while(I2CMasterBusy(I2C0_MASTER_BASE));
 buff[i] = I2CMasterDataGet(I2C_MASTER_BASE);
 }
 I2CMasterControl(I2C0_MASTER_BASE, I2C_MASTER_CMD_BURST_RECEIVE_FINISH);
 while(I2CMasterBusy(I2C0_MASTER_BASE));

 _gyroZ = ((buff[3] >> 2) << 8) + buff[0] - _gyroZ0;
 _gyroX = ((buff[4] >> 2) << 8) + buff[1] - _gyroX0;
 _gyroY = ((buff[5] >> 2) << 8) + buff[2] - _gyroY0;
}

void calibrateWMPGyrosZeroes(){
 char buf[25];
 _gyroX0 = 0; _gyroY0 = 0; _gyroZ0 = 0;
 long tempX=0, tempY=0, tempZ=0;
 int i;
 for (i=0; i<10; i++){
 readWMPGyros();

 tempZ += _gyroZ;
 tempX += _gyroX;
 tempY += _gyroY;
 }

 // average 10 readings
 _gyroZ0 = tempZ / 10;
 _gyroX0 = tempX / 10;
 _gyroY0 = tempY / 10;

 sprintf(buf, "X0:%d Y0:%d         ", _gyroX0, _gyroY0);
 RIT128x96x4StringDraw(buf, 0, 25, 11);
}

void taskA(void* pdata){
 char buf[25];

 // init the Wii Motion Plus. This is MANDATORY as the WMP starts at a different I2C address
 initWMPGyros();
 calibrateWMPGyrosZeroes();

 while(1){
 readWMPGyros();
 sprintf(buf, "X:%d Y:%d      ", _gyroX, _gyroY);
 RIT128x96x4StringDraw(buf, 0, 45, 11);
 CoTimeDelay(0, 0, 0, 300);
 }
}

int main(void){
 initBoard();

 CoInitOS();
 // name of the task method / NO arguments / priority 1 / the stack to use / stack size
 CoCreateTask(taskA, (void *)0, 1, &taskA_Stk[TASK_STK_SIZE - 1], TASK_STK_SIZE);
 CoStartOS();

 while(1);
}

As usual, don’t hesitate to post any comments, ask questions, or let me know if you found this useful at all…

About these ads

7 Responses to Wii Motion Plus gyros on LM3S8962 (I2C on CooCox)

  1. Pingback: Wii Motion plus Gyros on RaspberryPi | Robotics / Electronics / Physical Computing

  2. Hi,
    I tried CoCOX with Naze32, very nice but tasks not preemptive as I thought, I am sure that Naze32 code could be useful for you.
    Anyway I am not an expert in RTOS. but I could also port Multiwii to Arduino_DUE ….. http://technicaladventure.blogspot.com/2014/05/porting-multiwii-to-arduino-due.html

  3. TA says:

    Great projects!
    Thanks so much!

  4. Anton says:

    Thank you very much!

  5. Great program. I´m trying to use CoIDE but I´m having some difficulties how to use its sintaxe to set configurations (like core clock, define pins, etc). Yf you have more examples, could you send me? Thanks a lot.

    Wagner

    • trandi says:

      Hi Wagner and thank you for your comment.

      Sadly I have very little experience with this board and CoIDE too, about as much as it’s on this post really… :)

      Last week-end I tried to write a program to decode some PPM signals from a RC remote, but found it very frustrating as I couldn’t make the external interrupts work !
      And NO example on the net whatsoever !!!

      That’s when you realise that the power of the little Arduino, has nothing to do with the chipset or the (rather crappy IDE :) ) etc. … but with the huuuuge community around it… there’s literally NO subject or project that hasn’t been tried by (plenty of) other people before you…

      So sorry for this, I can’t really help, but I’d be curious to know if you manage to find any other examples.

      dan

  6. Pingback: Wii Accessories – We've Come So Far Since the Power Glove

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

Follow

Get every new post delivered to your Inbox.

Join 150 other followers

%d bloggers like this: