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;
}