Nyko Kama nunchuck controlled Zumo

Support Robots
1

This has been my first project and has gone well after tinkering around with lots of nunchuck codes. I have finally gotten the Zumo working well, except for one problem: after turning the Zumo on, and turning the Nyko on, immediately the left motor runs forward full speed until I hit the Reset button. Then all works well. I can’t figure out how to initially set both motors at no speed. I am using the ZumoMotors library and modified sketches.

Here is my code:

// read out a Wii Nunchuck controller
// adapted to work with wireless Nunchuck controllers of third party vendors by Michael Dreher

// adapt to your hardware config
//#define POWER_VIA_PORT_C2_C3 1 // use port pins port C2 and C3 as power supply of the Nunchuck (direct plug using wiichuck adapter)
//#define DEBUG_RCV_TEL 1

#define USE_NEW_WAY_INIT 1 // use "The New Way" of initialization <http://wiibrew.org/wiki/Wiimote#The_New_Way>
#define WII_IDENT_LEN ((byte)6)
#define WII_TELEGRAM_LEN ((byte)6)
#define WII_NUNCHUCK_TWI_ADR ((byte)0x52)
#define CPU_FREQ (16000000L)
#define LED_PIN 13

#include <Wire.h>
#include <string.h>
#include <utility\twi.h>
#undef int
#include <stdio.h>
#include <ZumoMotors.h>

ZumoMotors motors;

uint8_t ctrlr_type[6];
uint8_t outbuf[WII_TELEGRAM_LEN]; // array to store arduino output
int cnt = 0;
int ledPin = 13;
int X = 131, Y = 131; // default values 131 for center
int Xspeed = 0, Yspeed = 0; //motor speed default 0 for full stop
const int Xdeadzone = 3, Ydeadzone = 3; //amount of slack needed


void setup ()
{
  Serial.begin (19200);
  nunchuckInit();
  pinMode(ledPin, OUTPUT);
}

void loop ()
{
  Wire.requestFrom (WII_NUNCHUCK_TWI_ADR, WII_TELEGRAM_LEN); // request data from nunchuck
  
  for (cnt = 0; (cnt < WII_TELEGRAM_LEN) && Wire.available (); cnt++)
  {
    outbuf[cnt] = nunchuk_decode_byte (Wire.read ()); // receive byte as an integer
    digitalWrite (ledPin, HIGH); // sets the LED on
  }
  
  /* debugging
  #ifdef DEBUG_RCV_TEL
    Serial.print("avail=");
    Serial.print(Wire.available());
    Serial.print(" cnt=");
    Serial.println(cnt);
  #endif
  */
  
  clearTwiInputBuffer();
  
  // If we recieved the 6 bytes, then go print them
  if (cnt >= WII_TELEGRAM_LEN)
  {
    printandmove ();
  }
  
  send_zero (); // send the request for next bytes
  delay (20);
}



//
//******************Functions**************************
//

void nunchuckInit()
{
  byte cnt;
  
  Wire.begin();
              
  // init controller
  delay(1);
  Wire.beginTransmission(0x52);      // device address
  Wire.write(0xF0);                    // 1st initialisation register
  Wire.write(0x55);                    // 1st initialisation value
  Wire.endTransmission();
  delay(1);
  Wire.beginTransmission(0x52);
  Wire.write(0xFB);                    // 2nd initialisation register
  Wire.write(0x00);                    // 2nd initialisation value
  Wire.endTransmission();
  delay(1);
              
  // read the extension type from the register block        
  Wire.beginTransmission(0x52);
  Wire.write(0xFA);                    // extension type register
  Wire.endTransmission();
  Wire.beginTransmission(0x52);
  Wire.requestFrom(0x52, 6);               // request data from controller
  for (cnt = 0; cnt < 6; cnt++) {
      if (Wire.available()) {
          ctrlr_type[cnt] = Wire.read(); // Should be 0x0000 A420 0101 for Classic Controller, 0x0000 A420 0000 for nunchuck
      }
  }
  Wire.endTransmission();
  delay(1);
              
  // send the crypto key (zeros), in 3 blocks of 6, 6 & 4.
  Wire.beginTransmission(0x52);
  Wire.write(0xF0);                    // crypto key command register
  Wire.write(0xAA);                    // sends crypto enable notice
  Wire.endTransmission();
  delay(1);
  Wire.beginTransmission(0x52);
  Wire.write(0x40);                    // crypto key data address
  for (cnt = 0; cnt < 6; cnt++) {
      Wire.write(0x00);                    // sends 1st key block (zeros)
  }
  Wire.endTransmission();
  Wire.beginTransmission(0x52);
  Wire.write(0x40);                    // sends memory address
  for (cnt = 6; cnt < 12; cnt++) {
      Wire.write(0x00);                    // sends 2nd key block (zeros)
  }
  Wire.endTransmission();
  Wire.beginTransmission(0x52);
  Wire.write(0x40);                    // sends memory address
  for (cnt = 12; cnt < 16; cnt++) {
      Wire.write(0x00);                    // sends 3rd key block (zeros)
  }
  Wire.endTransmission();
  Serial.print ("Initiation success!");
  delay(1);
}


void clearTwiInputBuffer(void)
{
  // clear the receive buffer from any partial data
  while( Wire.available ())
  Wire.read ();
}


void send_zero ()
{
  // I don't know why, but it only works correct when doing this exactly 3 times
  // otherwise only each 3rd call reads data from the controller (cnt will be 0 the other times)
  for(byte i = 0; i < 3; i++)
  {
    Wire.beginTransmission (WII_NUNCHUCK_TWI_ADR); // transmit to device 0x52
    Wire.write (0x00); // sends one byte
    Wire.endTransmission (); // stop transmitting
  }
}

 
// Print the input data we have recieved
// accel data is 10 bits long
// so we read 8 bits, then we have to add
// on the last 2 bits. That is why I
// multiply them by 2 * 2
void printandmove ()
{
  int joy_x_axis = outbuf[0];
  int joy_y_axis = outbuf[1];
  int accel_x_axis = outbuf[2] * 2 * 2;
  int accel_y_axis = outbuf[3] * 2 * 2;
  int accel_z_axis = outbuf[4] * 2 * 2;
  
  int z_button = 0;
  int c_button = 0;
  
  // byte outbuf[5] contains bits for z and c buttons
  // it also contains the least significant bits for the accelerometer data
  // so we have to check each bit of byte outbuf[5]
  if ((outbuf[5] >> 0) & 1) {
    z_button = 1;
  }
  if ((outbuf[5] >> 1) & 1) {
    c_button = 1;
  }
  
  if ((outbuf[5] >> 2) & 1) {
    accel_x_axis += 2;
  }
  if ((outbuf[5] >> 3) & 1) {
    accel_x_axis += 1;
  }
  
  if ((outbuf[5] >> 4) & 1) {
    accel_y_axis += 2;
  }
  if ((outbuf[5] >> 5) & 1) {
    accel_y_axis += 1;
  }
  
  if ((outbuf[5] >> 6) & 1) {
    accel_z_axis += 2;
  }
  if ((outbuf[5] >> 7) & 1) {
    accel_z_axis += 1;
  }
  
  /*
  Serial.print (joy_x_axis, DEC);
  Serial.print ("\t");
  Serial.print (joy_y_axis, DEC);
  Serial.print ("\t");
  Serial.print (accel_x_axis, DEC);
  Serial.print ("\t");
  Serial.print (accel_y_axis, DEC);
  Serial.print ("\t");
  Serial.print (accel_z_axis, DEC);
  Serial.print ("\t");
  Serial.print (z_button, DEC);
  Serial.print ("\t");
  Serial.print (c_button, DEC);
  Serial.print ("\t");
  Serial.print ("\r\n");
  */
  
  //Now move
  int Xhi = 131, Xlo = 131, Yhi = 131, Ylo = 131;
  Xhi = 131 + Xdeadzone; 
  Xlo = 131 - Xdeadzone; 
  Yhi = 131 + Ydeadzone; 
  Ylo = 131 - Ydeadzone; 

  if (joy_y_axis > Yhi)//Forward
  { 
    digitalWrite(ledPin, HIGH);
    Yspeed = map(joy_y_axis, Yhi, 255, 0, 400);
    if(joy_x_axis < Xlo)                       //Left
    {
      Xspeed = map(joy_x_axis, 0, Xlo, 0, Yspeed); 
      motors.setLeftSpeed(Xspeed); 
      motors.setRightSpeed(Yspeed);
      //Serial.print (Yspeed); Serial.print ("\t"); Serial.print(Xspeed); Serial.print ("\t\r\n");
    }
    else if (joy_x_axis > Xhi)                  //Right
    {
      Xspeed = map(joy_x_axis, Xhi, 255, -Yspeed, 0);
        Xspeed = abs(Xspeed);
      motors.setLeftSpeed(Yspeed);
      motors.setRightSpeed(Xspeed);
      //Serial.print (Yspeed); Serial.print ("\t"); Serial.print(Xspeed); Serial.print ("\t\r\n");
    }
    else  //Straight
    {
      motors.setSpeeds(Yspeed, Yspeed); 
      //Serial.print (Yspeed); Serial.print ("\t"); Serial.print(Xspeed); Serial.print ("\t\r\n");
    }
  }

  else if (joy_y_axis < Ylo)//Reverse
  { 
    digitalWrite(ledPin, HIGH);
    Yspeed = map(joy_y_axis, 0, Ylo, -400, 0);
    if(joy_x_axis < Xlo)                        //Left
    {
      Xspeed = map(joy_x_axis, 0, Xlo, Yspeed, 0);
      motors.setLeftSpeed(Xspeed); 
      motors.setRightSpeed(Yspeed);
      //Serial.print (Yspeed); Serial.print ("\t"); Serial.print(Xspeed); Serial.print ("\t\r\n");
    }
    else if (joy_x_axis > Xhi) //Right
    {
      Xspeed = map(joy_x_axis, Xhi, 255, Yspeed, 0);
      motors.setLeftSpeed(Yspeed);
      motors.setRightSpeed(Xspeed);
      //Serial.print (Yspeed); Serial.print ("\t"); Serial.print(Xspeed); Serial.print ("\t\r\n");
    }
    else //Straight
    {
      motors.setSpeeds(Yspeed, Yspeed);
      //Serial.print (Yspeed); Serial.print ("\t"); Serial.print(Xspeed); Serial.print ("\t\r\n");
    }
  }
  
  else // X is between 131 +- deadzone
  {
    Yspeed = 0;
    if(joy_x_axis >= Xhi) // zero point turn Right
    {
      digitalWrite(ledPin, HIGH);
      Xspeed = map(joy_x_axis, Xhi, 255, 0, 200);
      motors.setLeftSpeed(Xspeed); 
      motors.setRightSpeed(Yspeed);
      //Serial.print (Yspeed); Serial.print ("\t"); Serial.print(Xspeed); Serial.print ("\t\r\n");
    }
    else if(joy_x_axis <= Xlo)// zero point turn Left
    {
      digitalWrite(ledPin, HIGH);
      Xspeed = map(joy_x_axis, 0, Xlo, 0, 200);
      motors.setLeftSpeed(Yspeed); 
      motors.setRightSpeed(Xspeed);
      //Serial.print (Yspeed); Serial.print ("\t"); Serial.print(Xspeed); Serial.print ("\t\r\n");	  
    }
    else
    { 							// Full stop
      digitalWrite(ledPin, LOW);
      Xspeed = 0;
      motors.setSpeeds(Yspeed, Xspeed);
      //Serial.print (Yspeed); Serial.print ("\t"); Serial.print(Xspeed); Serial.print ("\t\r\n");
    }
  }
}

// Decode data format that original Nunchuck uses with old init sequence. This never worked with
// other controllers (e.g. wireless Nunchuck from other vendors)
char nunchuk_decode_byte (char x)
{
  #ifndef USE_NEW_WAY_INIT
    x = (x ^ 0x17) + 0x17;
  #endif
  return x;
}

I would greatly appreciate any advice or suggestions.

Kris

2

Hello, Kris.

That sounds like a really cool project! I would love to see a video of it.

It might be that the nunchuck is not getting enough time to fully initialize. You could try adding a delay to your code after nunchuckInit() to give the nunchuck enough time to initialize before you query it. If that does not work, I suggest creating a new sketch and building your code back up again piece by piece until you find the part that is causing the problem. If you are still having trouble at that point, you might try posting a simplified version of your code. That might make it easier to see potential issues.

- Grant

3

Grant,
Thanks for your reply. The delay didn’t end up having much noticeable effect, but I finally realized that I just needed to reinitialize the nunchuck again just after it had been initially initialized, then all works well.

Here is the final code that seems to work well:

// read out a Wii Nunchuck controller and control Zumo
// adapted from code at: http://www.windmeadow.com/node/42
// adapted to work with wireless Nunchuck controllers of third party vendors by Michael Dreher
// further adapted and modified for Zumo by Kris Andersen

// adapt to your hardware config
//#define POWER_VIA_PORT_C2_C3 1 // use port pins port C2 and C3 as power supply of the Nunchuck (direct plug using wiichuck adapter)
//#define DEBUG_RCV_TEL 1

//#define USE_NEW_WAY_INIT 1 // use "The New Way" of initialization <http://wiibrew.org/wiki/Wiimote#The_New_Way>
#define CPU_FREQ (16000000L)
#define LED_PIN 13

#include <Wire.h>
#include <string.h>
#include <utility\twi.h>
#undef int
#include <ZumoMotors.h>

ZumoMotors motors;

uint8_t ctrlr_type[6];
uint8_t outbuf[6]; // array to store arduino output
byte cnt = 0;
const int ledPin = 13;
int Xspeed = 0, Yspeed = 0;             //motor speed default 0 for full stop
const int Xdeadzone = 3, Ydeadzone = 3; //amount of slack needed
unsigned int dataSetCnt = 0;            //counter for data  sets received via i2c

void setup () {
  pinMode(ledPin, OUTPUT);
  //Serial.begin (19200);  //uncomment for debugging
  nunchuckInit();
}

void loop () {
  Wire.requestFrom (0x52, 6); // request data from nunchuck
  for (cnt = 0; (cnt < 6) && Wire.available (); cnt++) {
    outbuf[cnt] = Wire.read ();       // receive byte as an integer
    digitalWrite (ledPin, HIGH);      // turns the LED on
  }
  /* debugging
  #ifdef DEBUG_RCV_TEL
    Serial.print("avail=");
    Serial.print(Wire.available());
    Serial.print(" cnt=");
    Serial.println(cnt);
  #endif
  */
  clearTwiInputBuffer();
  if (cnt >= 6) {    // If we received the 6 bytes, then go print them
    dataSetCnt = dataSetCnt++;
    if (dataSetCnt == 1) { //reinitialize in order to receive correct data
      nunchuckInit();
    }    
    if (nunchuck_cbutton() && nunchuck_zbutton()) {  //if both z and c button pressed
      delay (1000);
      nunchuckInit();  //reinitialize nunchuck
      delay (500);
    }
    
    if (dataSetCnt > 65530) {     //roll back counter starting at 2  
      dataSetCnt = 2;
    }
    //Serial.println(dataSetCnt);
    moveNunchuck ();
  }
  send_zero (); // send the request for next bytes
  delay (20);
}



//
//******************Functions**************************
//

void nunchuckInit()
{
  byte cnt;
  
  Wire.begin();   //join i2c bus as master
              
  // init controller
  delay(1);
  Wire.beginTransmission(0x52);      // transmit to device address
  Wire.write(0xF0);                    // 1st initialisation register
  Wire.write(0x55);                    // 1st initialisation value
  Wire.endTransmission();
  delay(1);
  Wire.beginTransmission(0x52);
  Wire.write(0xFB);                    // 2nd initialisation register
  Wire.write(0x00);                    // 2nd initialisation value
  Wire.endTransmission();
  delay(1);
              
  // read the extension type from the register block        
  Wire.beginTransmission(0x52);
  Wire.write(0xFA);                    // extension type register
  Wire.endTransmission();
  Wire.beginTransmission(0x52);
  Wire.requestFrom(0x52, 6);               // request data from controller
  for (cnt = 0; cnt < 6; cnt++) {
      if (Wire.available()) {
          ctrlr_type[cnt] = Wire.read(); // Should be 0x0000 A420 0101 for Classic Controller, 0x0000 A420 0000 for nunchuck
      }
  }
  Wire.endTransmission();
  delay(1);
              
  // send the crypto key (zeros), in 3 blocks of 6, 6 & 4.
  Wire.beginTransmission(0x52);
  Wire.write(0xF0);                    // crypto key command register
  Wire.write(0xAA);                    // sends crypto enable notice
  Wire.endTransmission();
  delay(1);
  Wire.beginTransmission(0x52);
  Wire.write(0x40);                    // crypto key data address
  for (cnt = 0; cnt < 6; cnt++) {
      Wire.write(0x00);                    // sends 1st key block (zeros)
  }
  Wire.endTransmission();
  Wire.beginTransmission(0x52);
  Wire.write(0x40);                    // sends memory address
  for (cnt = 6; cnt < 12; cnt++) {
      Wire.write(0x00);                    // sends 2nd key block (zeros)
  }
  Wire.endTransmission();
  Wire.beginTransmission(0x52);
  Wire.write(0x40);                    // sends memory address
  for (cnt = 12; cnt < 16; cnt++) {
      Wire.write(0x00);                    // sends 3rd key block (zeros)
  }
  Wire.endTransmission();
  Serial.print ("Initiation complete!");
  delay(1);
}


void clearTwiInputBuffer(void)  //// clear the receive buffer from any partial data
{
  while(Wire.available ())
  Wire.read ();
}


void send_zero ()
{
  // I don't know why, but it only works correct when doing this exactly 3 times
  // otherwise only each 3rd call reads data from the controller (cnt will be 0 the other times)
  for(byte i = 0; i < 3; i++) {
    Wire.beginTransmission (0x52); // transmit to device 0x52
    Wire.write ((byte)0x00); // sends one byte
    Wire.endTransmission (); // stop transmitting
  }
}

 
// Print the input data we have received
// accel data is 10 bits long
// so we read 8 bits, then we have to add
// on the last 2 bits. That is why I
// multiply them by 2 * 2
void moveNunchuck ()
{
  int joy_x_axis = outbuf[0];
  int joy_y_axis = outbuf[1];
  int accel_x_axis = outbuf[2] * 2 * 2;
  int accel_y_axis = outbuf[3] * 2 * 2;
  int accel_z_axis = outbuf[4] * 2 * 2;
  
  int z_button = 0;
  int c_button = 0;
  
  // byte outbuf[5] contains bits for z and c buttons
  // it also contains the least significant bits for the accelerometer data
  // so we have to check each bit of byte outbuf[5]
  if ((outbuf[5] >> 0) & 1) {  //if last bit is 1
    z_button = 1;
  }
  if ((outbuf[5] >> 1) & 1) {  //if second to the last bit is 1
    c_button = 1;
  }
  
  /*
  if ((outbuf[5] >> 2) & 1) {
    accel_x_axis += 2;
  }
  if ((outbuf[5] >> 3) & 1) {
    accel_x_axis += 1;
  }
  
  if ((outbuf[5] >> 4) & 1) {
    accel_y_axis += 2;
  }
  if ((outbuf[5] >> 5) & 1) {
    accel_y_axis += 1;
  }
  
  if ((outbuf[5] >> 6) & 1) {
    accel_z_axis += 2;
  }
  if ((outbuf[5] >> 7) & 1) {
    accel_z_axis += 1;
  }
  */
 
  Serial.print (joy_x_axis, DEC);
  Serial.print ("\t");
  Serial.print (joy_y_axis, DEC);
  Serial.print ("\t");
  Serial.print (accel_x_axis, DEC);
  Serial.print ("\t");
  Serial.print (accel_y_axis, DEC);
  Serial.print ("\t");
  Serial.print (accel_z_axis, DEC);
  Serial.print ("\t");
  Serial.print (z_button, DEC);
  Serial.print ("\t");
  Serial.print (c_button, DEC);
  Serial.print ("\t");
  Serial.print ("\r\n");
  
  
  
  //Now move
  int Xhi = 131 + Xdeadzone; 
  int Xlo = 131 - Xdeadzone; 
  int Yhi = 131 + Ydeadzone; 
  int Ylo = 131 - Ydeadzone; 
  
  if (joy_y_axis == 255 && joy_x_axis == 255)// Zero invalid data sets
  {
    joy_y_axis = 131;
    joy_x_axis = 131;
  }
  
  if (joy_y_axis > Yhi) {  //Forward
    digitalWrite(ledPin, HIGH);
    Yspeed = map(joy_y_axis, Yhi, 255, 0, 400);
    if(joy_x_axis < Xlo) {                      //Left
      Xspeed = map(joy_x_axis, 0, Xlo, 0, Yspeed); 
      motors.setLeftSpeed(Xspeed); 
      motors.setRightSpeed(Yspeed);
      //Serial.print (Yspeed); Serial.print ("\t"); Serial.print(Xspeed); Serial.print ("\t\r\n");
    }
    else if (joy_x_axis > Xhi) {                //Right
      Xspeed = map(joy_x_axis, Xhi, 255, Yspeed, 0);
      motors.setLeftSpeed(Yspeed);
      motors.setRightSpeed(Xspeed);
      //Serial.print (Yspeed); Serial.print ("\t"); Serial.print(Xspeed); Serial.print ("\t\r\n");
    }
    else { //Straight
      motors.setSpeeds(Yspeed, Yspeed); 
      //Serial.print (Yspeed); Serial.print ("\t"); Serial.print(Xspeed); Serial.print ("\t\r\n");
    }
  }

  else if (joy_y_axis < Ylo)  {                 //Reverse
    digitalWrite(ledPin, HIGH);
    Yspeed = map(joy_y_axis, 0, Ylo, -400, 0);
    if(joy_x_axis < Xlo) {                       //Left
      Xspeed = map(joy_x_axis, 0, Xlo, 0, Yspeed);
      motors.setLeftSpeed(Xspeed); 
      motors.setRightSpeed(Yspeed);
      //Serial.print (Yspeed); Serial.print ("\t"); Serial.print(Xspeed); Serial.print ("\t\r\n");
    }
    else if (joy_x_axis > Xhi) {                  //Right
      Xspeed = map(joy_x_axis, Xhi, 255, Yspeed, 0);
      motors.setLeftSpeed(Yspeed);
      motors.setRightSpeed(Xspeed);
      //Serial.print (Yspeed); Serial.print ("\t"); Serial.print(Xspeed); Serial.print ("\t\r\n");
    }
    else {                                      //Straight
      motors.setSpeeds(Yspeed, Yspeed);
      //Serial.print (Yspeed); Serial.print ("\t"); Serial.print(Xspeed); Serial.print ("\t\r\n");
    }
  }
  
  else {                               // X is between 131 +- deadzone
    Yspeed = 0;
    if(joy_x_axis >= Xhi) {            // zero point turn Right
      digitalWrite(ledPin, HIGH);
      Xspeed = map(joy_x_axis, Xhi, 255, 0, 300);
      motors.setLeftSpeed(Xspeed); 
      motors.setRightSpeed(Yspeed);
      //Serial.print (Yspeed); Serial.print ("\t"); Serial.print(Xspeed); Serial.print ("\t\r\n");
    }
    else if(joy_x_axis <= Xlo) {       // zero point turn Left
      digitalWrite(ledPin, HIGH);
      Xspeed = map(joy_x_axis, 0, Xlo, 300, 0);
      motors.setLeftSpeed(Yspeed); 
      motors.setRightSpeed(Xspeed);
      //Serial.print (Yspeed); Serial.print ("\t"); Serial.print(Xspeed); Serial.print ("\t\r\n");	  
    }
    else { 			      // Full stop
      digitalWrite(ledPin, LOW);
      Xspeed = 0;
      motors.setSpeeds(Yspeed, Xspeed);
      //Serial.print (Yspeed); Serial.print ("\t"); Serial.print(Xspeed); Serial.print ("\t\r\n");
    }
  }
}

// returns zbutton state: 1=pressed, 0=notpressed
int nunchuck_zbutton()
{
   return ((outbuf[5] >> 0) & 1) ? 0 : 1;  // return value of last bit
}

// returns cbutton state: 1=pressed, 0=notpressed
int nunchuck_cbutton()
{
   return ((outbuf[5] >> 1) & 1) ? 0 : 1;  // return value of second to last bit
}

// Decode data format that original Nunchuck uses with old init sequence. This never worked with
// other controllers (e.g. wireless Nunchuck from other vendors)
//char nunchuk_decode_byte (char x) {
//  #ifndef USE_NEW_WAY_INIT
//    x = (x ^ 0x17) + 0x17;
//  #endif
//  return x;
//}

Kris

4

That’s great; I am glad you got it working! I would love to see a video of your nunchuck controlled Zumo in action.

You can add code blocks to your post by putting your code between the two code blocks “”. You can also generate these code blocks using the “Code” button above the text field where you enter the content for your post.

- Grant