Help with WII Chuck please

Projects Programming
1

Hi, Iv gone as far as I can by my self, Id like to make the joystick the primary input, and get an average reading off the Accel x,y,z, to help smooth out the movement of my servos. Iv been looking at this code for 2 weeks now and I'm stuck, any help would be grate.
Thanks
Wallaby

#include "Arduino.h"
#include "Wire.h"
#include "string.h"
#include "stdio.h"
//#define OUTPUT_SERIAL

#define min(a, b) ((a<b)?a:b)
#define max(a, b) ((a>b)?a:b)

// Servo control
class ServoControl
{
private:

  // Private variables
  int _pin;
  int _neutral;
  int _range;
  float _currentRotation;
  int _currentDelay;

public:

  // Constructor
  ServoControl(int pin, int neutral = 1500, int range = 900) :// Range was 500
  _pin(pin), _neutral(neutral), _range(range)
  {
  }

  // Initialize
  void Init()
  {
    pinMode(_pin, OUTPUT);
    digitalWrite(_pin, LOW);
    delay(1);
    digitalWrite(_pin, HIGH);
    delayMicroseconds(_neutral);
    digitalWrite(_pin, LOW);
  }

  // Set position [-1.0f, 1.0f]
  void SetRotation(float rot)
  {
    _currentRotation = rot;
    _currentDelay = (int)( (float)_neutral+_currentRotation*(float)_range );
  }

  // Update
  void Update()
  {
    digitalWrite(_pin, HIGH);
    delayMicroseconds(_currentDelay);
    digitalWrite(_pin, LOW);
  }

  // Accessors
  int GetNeutral() { 
    return _neutral; 
  }
  int GetRange() { 
    return _range; 
  }
  int GetLow() { 
    return _neutral-_range; 
  }
  int GetHigh() { 
    return _neutral+_range; 
  }
  float GetCurrentRotation() { 
    return _currentRotation; 
  }
  int GetCurrentDelay() { 
    return _currentDelay; 
  }

};

// Wii Nunchuck constants
#define xAnalogLo 27
#define xAnalogNu 122
#define xAnalogHi 220

#define yAnalogLo 28
#define yAnalogNu 126
#define yAnalogHi 226

#define xAccelLo 288
#define xAccelNu 500
#define xAccelHi 704

#define yAccelLo 280
#define yAccelNu 488
#define yAccelHi 700

#define zAccelLo 296
#define zAccelNu 504
#define zAccelHi 708

// Wii Nunchuck control
class WiiNunchuckControl
{
public:

  // Public variables
  unsigned int xAnalog;
  unsigned int yAnalog;
  unsigned int xAccel;
  unsigned int yAccel;
  unsigned int zAccel;
  bool zButtonDown;
  bool cButtonDown;
  bool zButtonHit;
  bool cButtonHit;

  // Constructor
  WiiNunchuckControl(int statusLED = 0, int cLED = 0, int zLED = 0) : 
  _statusLED(statusLED), _cLED(cLED), _zLED(zLED)
  {
  }

  // Initialize
  void Init()
  {
    if( _statusLED != 0 ) pinMode(_statusLED,OUTPUT);
    if( _cLED != 0 ) pinMode(_cLED, OUTPUT);
    if( _zLED != 0 ) pinMode(_zLED, OUTPUT);
    Wire.beginTransmission(0x52);
    Wire.write(0x40);
    Wire.write((byte)0x00);
    Wire.endTransmission();
  }

  // Read data from Nunchuck using Wire lib (I2C)
  void ReadDataFromNunchuck()
  {
    int count = 0;

    // Get data
    Wire.requestFrom(0x52, 6);
    digitalWrite(_statusLED, HIGH);
    while( Wire.available() )
    {
      _rawData[count] = DecodeByte( Wire.read() );
      digitalWrite(_statusLED, LOW);
      count++;
    }

    // Save it into public variables
    if( count >= 5 )
    {
      ExtractDataFromBuffer(_rawData);
    }

    // Send the request for next bytes
    SendZero();
  }

  // Get x analog as float
  float GetXAnalog()
  {
    if( xAnalog <= xAnalogNu )
      return ((float)xAnalog-xAnalogNu)/(xAnalogNu-xAnalogLo);
    else
      return ((float)xAnalog-xAnalogNu)/(xAnalogHi-xAnalogNu);
  }

  // Get y analog as float
  float GetYAnalog()
  {
    if( yAnalog <= yAnalogNu )
      return ((float)yAnalog-yAnalogNu)/(yAnalogNu-yAnalogLo);
    else
      return ((float)yAnalog-yAnalogNu)/(yAnalogHi-yAnalogNu);
  }

  // Get x acceleration as float
  float GetXAccel()
  {
    if( xAccel <= xAccelNu )
      return ((float)xAccel-xAccelNu)/(xAccelNu-xAccelLo);
    else
      return ((float)xAccel-xAccelNu)/(xAccelHi-xAccelNu);
  }

  // Get y acceleration as float
  float GetYAccel()
  {
    if( yAccel <= yAccelNu )
      return ((float)yAccel-yAccelNu)/(yAccelNu-yAccelLo);
    else
      return ((float)yAccel-yAccelNu)/(yAccelHi-yAccelNu);
  }

  // Get z acceleration as float
  float GetZAccel()
  {
    if( zAccel <= zAccelNu )
      return ((float)zAccel-zAccelNu)/(zAccelNu-zAccelLo);
    else
      return ((float)zAccel-zAccelNu)/(zAccelHi-zAccelNu);
  }

private:

  // Private variables
  int _statusLED, _cLED, _zLED;
  unsigned int _rawData[6];

  // Decode nunchuck data per byte
  unsigned int DecodeByte(unsigned int x)
  {
    x = (x^0x17)+0x17;
    return x;
  }

  // Send zero to reset data transfer
  void SendZero()
  {
    Wire.beginTransmission(0x52);
    Wire.write((byte)0x00);
    Wire.endTransmission();
  }

  // Extract data from data buffer
  void ExtractDataFromBuffer(unsigned int* buf)
  {
    // Read z button
    bool zButton = !( (buf[5] >> 0) & 1 );
    zButtonHit = false;
    if( zButton && !zButtonDown ) zButtonHit = true;
    zButtonDown = zButton;
    if( zButton && _zLED != 0 ) digitalWrite(_zLED, LOW);
    else digitalWrite(_zLED, HIGH);

    // Read c button
    bool cButton = !( (buf[5] >> 1) & 1 );
    cButtonHit = false;
    if( cButton && !cButtonDown ) cButtonHit = true;
    cButtonDown = cButton;
    if( cButton && _cLED != 0 ) digitalWrite(_cLED, LOW);
    else digitalWrite(_cLED, HIGH);

    // Read analog data
    xAnalog = buf[0];
    yAnalog = buf[1];
    xAccel = buf[2] * 2 * 2; 
    yAccel = buf[3] * 2 * 2;
    zAccel = buf[4] * 2 * 2;

    // Add significant bits
    if( (buf[5] >> 2) & 1 ) xAccel += 2;
    if( (buf[5] >> 3) & 1 ) xAccel += 1;
    if( (buf[5] >> 4) & 1 ) yAccel += 2;
    if( (buf[5] >> 5) & 1 ) yAccel += 1;
    if( (buf[5] >> 6) & 1 ) zAccel += 2;
    if( (buf[5] >> 7) & 1 ) zAccel += 1;
  }

};

ServoControl yawServo(7);
ServoControl pitchServo(2);
WiiNunchuckControl myWiiNunchuck(3, 5, 6);

// Smoothing macros
#define MAX_SMOOTHING_LEVELS 7
#define WrapIdx(i) (i+MAX_SMOOTHING_LEVELS)%MAX_SMOOTHING_LEVELS

// Smoothing data
unsigned int smoothingLevel = 0;
unsigned int lastAnalogIdx = 0;
float lastXAnalogPos[MAX_SMOOTHING_LEVELS];
float lastYAnalogPos[MAX_SMOOTHING_LEVELS];

// Program setup
void setup() 
{ 
  // Initialize libraries
  Wire.begin();
#ifdef OUTPUT_SERIAL
  beginSerial(19200);
#endif

  // Initialize servo control
  yawServo.Init();
  pitchServo.Init();

  // Initialize Wii Nunchuck control
  myWiiNunchuck.Init();

  // Initialize smoothing LEDs
  pinMode(8, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);

  delay(1000);
} 

// Program loop
void loop() 
{ 

  // Prepare smoothing data
  float totalLastXAnalogPos = 0;
  float totalLastYAnalogPos = 0;
  for( char i = 0; i < smoothingLevel; ++i )
  {
    char idx = WrapIdx(lastAnalogIdx-(i+1));
    totalLastXAnalogPos += lastXAnalogPos[idx];
    totalLastYAnalogPos += lastYAnalogPos[idx];
  }

  // Read nunchuck data and smooth position
  myWiiNunchuck.ReadDataFromNunchuck();
  float xAnalogPos, yAnalogPos;
  if( myWiiNunchuck.zButtonDown && myWiiNunchuck.cButtonDown )
  {
    xAnalogPos = ( myWiiNunchuck.GetXAnalog()+totalLastXAnalogPos )/(float)(smoothingLevel+1);
    yAnalogPos = ( myWiiNunchuck.GetYAnalog()+totalLastYAnalogPos )/(float)(smoothingLevel+1);
  }
  else
  {
    xAnalogPos = ( min(max(myWiiNunchuck.GetXAccel()*1.25f, -1.0f), 1.0f)+totalLastXAnalogPos )/(float)(smoothingLevel+1);
    yAnalogPos = ( min(max(myWiiNunchuck.GetYAccel()*1.5f, -1.0f), 1.0f)+totalLastYAnalogPos )/(float)(smoothingLevel+1);
  }

  // Update smoothing data
  lastXAnalogPos[lastAnalogIdx] = xAnalogPos;
  lastYAnalogPos[lastAnalogIdx] = yAnalogPos;
  lastAnalogIdx = WrapIdx(lastAnalogIdx+1);

  // Change smoothing level
  if( myWiiNunchuck.cButtonHit ) smoothingLevel = max(smoothingLevel, 1)-1;
  if( myWiiNunchuck.zButtonHit ) smoothingLevel = min(smoothingLevel+1, MAX_SMOOTHING_LEVELS);
  // Display smoothing level
  digitalWrite(8, (smoothingLevel >> 0) & 1);
  digitalWrite(9, (smoothingLevel >> 1) & 1);
  digitalWrite(10, (smoothingLevel >> 2) & 1);

  // Check buttons
  if( myWiiNunchuck.zButtonDown && ( myWiiNunchuck.zButtonDown != myWiiNunchuck.cButtonDown ) )
    yawServo.SetRotation(yawServo.GetCurrentRotation());
  else
    yawServo.SetRotation(xAnalogPos);

  if( myWiiNunchuck.cButtonDown && ( myWiiNunchuck.zButtonDown != myWiiNunchuck.cButtonDown ) )
    pitchServo.SetRotation(pitchServo.GetCurrentRotation());
  else
    pitchServo.SetRotation(yAnalogPos);

  // Update servos
  yawServo.Update();
  pitchServo.Update();
    
#ifdef OUTPUT_SERIAL
    
  Serial.print(myWiiNunchuck.xAnalog, DEC);
  Serial.print("\t");
  Serial.print(myWiiNunchuck.yAnalog, DEC);
  Serial.print("\t");
  Serial.print(myWiiNunchuck.xAccel, DEC);
  Serial.print("\t");
  Serial.print(myWiiNunchuck.yAccel, DEC);
  Serial.print("\t");
  Serial.print(myWiiNunchuck.zAccel, DEC);
  Serial.print("\n");
  delay(5);
#endif

#ifndef OUTPUT_SERIAL
  delay(15);
#endif

} 

int main(void)
{
    init();

    setup();
    
    for (;;)
        loop();
        
    return 0;
}
2

What is the problem? Primary input? You want too read it more than the other controls?!.
Why have you stopped the core version of int main from running. You use the serial library and the core version does things yours doesn't

if (serialEventRun) serialEventRun();
3

The code I got online, I had to do some research to get it to work but it mostly does what i want it to do now. Im new to coding and have no idea what most of it does or doesn't do :~
The accelerometer is the primary input, if you hold down the c and z buttons together control goes to the joystick, Id like to switch that but i can't find it in the code. Also the accelerometer is too sensitive id like to get like 5 or more readings from it and average them to make it a bit smoother but i don't know how or where to to that.

Thanks for replying
Wallaby

4

this is the statement that says read the joystick when Z & c is down otherwise read accelerometer, switch their contents.

  if( myWiiNunchuck.zButtonDown && myWiiNunchuck.cButtonDown )
  {
    xAnalogPos = ( myWiiNunchuck.GetXAnalog()+totalLastXAnalogPos )/(float)(smoothingLevel+1);
    yAnalogPos = ( myWiiNunchuck.GetYAnalog()+totalLastYAnalogPos )/(float)(smoothingLevel+1);
  }
  else
  {
    xAnalogPos = ( min(max(myWiiNunchuck.GetXAccel()*1.25f, -1.0f), 1.0f)+totalLastXAnalogPos )/(float)(smoothingLevel+1);
    yAnalogPos = ( min(max(myWiiNunchuck.GetYAccel()*1.5f, -1.0f), 1.0f)+totalLastYAnalogPos )/(float)(smoothingLevel+1);
  }

too

  if( myWiiNunchuck.zButtonDown && myWiiNunchuck.cButtonDown )
  {
    xAnalogPos = ( min(max(myWiiNunchuck.GetXAccel()*1.25f, -1.0f), 1.0f)+totalLastXAnalogPos )/(float)(smoothingLevel+1);
    yAnalogPos = ( min(max(myWiiNunchuck.GetYAccel()*1.5f, -1.0f), 1.0f)+totalLastYAnalogPos )/(float)(smoothingLevel+1);
  }
  else
  {
    xAnalogPos = ( myWiiNunchuck.GetXAnalog()+totalLastXAnalogPos )/(float)(smoothingLevel+1);
    yAnalogPos = ( myWiiNunchuck.GetYAnalog()+totalLastYAnalogPos )/(float)(smoothingLevel+1);
  }

It looks like the servo control is bound to the buttons as well, that may need altering also.

5

There is 7 "smoothing" stages controlled by the c and z button, it seams to just make the servos move slower, as well if you hold the c button the locks one servo leaving the other to still move using the accelerometer, same goes for the z button.

So should I switch all the Analog to Accel, and Accel to Analog? And what does float mean in this?

// Get x analog as float
  float GetXAnalog()
  {
    if( xAnalog <= xAnalogNu )
      return ((float)xAnalog-xAnalogNu)/(xAnalogNu-xAnalogLo);
    else
      return ((float)xAnalog-xAnalogNu)/(xAnalogHi-xAnalogNu);

Thanks for the help

6

No that function is called by the joystick code.
The class functions are fine how they are.

xAnalogPos = ( myWiiNunchuck.GetXAnalog()+totalLastXAnalogPos )/(float)(smoothingLevel+1);

float is used in a cast, xAnalog-xAnalogNu is converted to a floating type so the '/' uses decimal precision ( keeps decimal places ) rather than integer precision ( no decimal places ).

Code like this may need fixing

  // Check buttons
  if( myWiiNunchuck.zButtonDown && ( myWiiNunchuck.zButtonDown != myWiiNunchuck.cButtonDown ) )
    yawServo.SetRotation(yawServo.GetCurrentRotation());
  else
    yawServo.SetRotation(xAnalogPos);

as with the c,z buttons now reversed, this checks that z is down and c isn't but the accelerometer gets read when c and z are both down.

also, don't confuse yourself, that if statement above is the same as

if( myWiiNunchuck.zButtonDown && !myWiiNunchuck.cButtonDown )
7

Ok so I changed this:

// Check buttons
  if( myWiiNunchuck.zButtonDown && !myWiiNunchuck.cButtonDown )
    yawServo.SetRotation(yawServo.GetCurrentRotation());
  else
    yawServo.SetRotation(xAnalogPos);

  if( myWiiNunchuck.cButtonDown && !myWiiNunchuck.zButtonDown )
    pitchServo.SetRotation(pitchServo.GetCurrentRotation());
  else
    pitchServo.SetRotation(yAnalogPos);

And tested and it works the same. Thank you for that.

I still don't get how to swap the accelerometer and joystick, Im kind of dumb when it comes to code. Iv tried to figure out how it get the order (or priority) of the accelerometer and joystick but i can't find it :frowning:

// Read nunchuck data and smooth position
  myWiiNunchuck.ReadDataFromNunchuck();
  float xAnalogPos, yAnalogPos;
  if( myWiiNunchuck.zButtonDown && myWiiNunchuck.cButtonDown )
  {
    xAnalogPos = ( myWiiNunchuck.GetXAnalog()+totalLastXAnalogPos )/(float)(smoothingLevel+1);
    yAnalogPos = ( myWiiNunchuck.GetYAnalog()+totalLastYAnalogPos )/(float)(smoothingLevel+1);
  }
  else
  {
    xAnalogPos = ( min(max(myWiiNunchuck.GetXAccel()*1.25f, -1.0f), 1.0f)+totalLastXAnalogPos )/(float)(smoothingLevel+1);
    yAnalogPos = ( min(max(myWiiNunchuck.GetYAccel()*1.5f, -1.0f), 1.0f)+totalLastYAnalogPos )/(float)(smoothingLevel+1);
  }

Im guessing its in here? Am I kind of right?

Thanks for your time.