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