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…