FireFly 2012: the robot code as an Arduino library 'hexapod' (v3.1): IK, SSC-32, IR, ultrasonic, GPS, 7-segment

From Electronics\Hexapod\Arduino library 2012.

2011-firefly-mega-ssc32/firmware/2012-library/hexapod/WiiChuck.h

@@ -0,0 +1,190 @@
+#ifndef WiiChuck_h
+#define WiiChuck_h
+
+#include "Arduino.h" 
+#include "Wire.h"
+#include "math.h"
+
+
+// these may need to be adjusted for each nunchuck for calibration
+#define ZEROX 510  
+#define ZEROY 490
+#define ZEROZ 460
+#define RADIUS 210  // probably pretty universal
+
+#define DEFAULT_ZERO_JOY_X 124
+#define DEFAULT_ZERO_JOY_Y 132
+
+#define MAXANGLE 90
+#define MINANGLE -90
+
+class WiiChuck {
+private:
+  byte cnt;
+  uint8_t status[6];		// array to store wiichuck output
+  byte averageCounter;
+  int i;
+  int total;
+  uint8_t zeroJoyX;   // these are about where mine are
+  uint8_t zeroJoyY; // use calibrateJoy when the stick is at zero to correct
+  int lastJoyX;
+  int lastJoyY;
+  int angles[3];
+
+  boolean lastZ, lastC;
+
+
+public:
+
+  byte joyX;
+  byte joyY;
+  boolean buttonZ;
+  boolean buttonC;
+  boolean plugd;
+  void begin() 
+  {
+    Wire.begin();
+    cnt = 0;
+    averageCounter = 0;
+    Wire.beginTransmission (0x52);	// transmit to device 0x52
+    Wire.write((byte)0x40);		// sends memory address
+    Wire.write((byte)0x00);		// sends memory address
+    Wire.endTransmission ();	// stop transmitting
+    update();            
+    for (i = 0; i<3;i++) {
+      angles[i] = 0;
+    }
+    zeroJoyX = DEFAULT_ZERO_JOY_X;
+    zeroJoyY = DEFAULT_ZERO_JOY_Y;
+  }
+
+
+  void calibrateJoy() {
+    zeroJoyX = joyX;
+    zeroJoyY = joyY;
+  }
+
+  void CheckifPlugd()
+  {
+    if (this->plugd == false){
+      this->begin();
+      
+#ifdef GLCD
+      GLCD.CursorTo(1, 1);
+      GLCD.print("No Chuck");
+      delay(100);
+#endif
+
+      this->begin();
+      if (this->plugd)
+        this->calibrateJoy(); 
+    }
+  }
+
+  void update() {
+
+    Wire.requestFrom (0x52, 6);	// request data from nunchuck
+    while (Wire.available ()) {
+      // receive byte as an integer
+      status[cnt] = _nunchuk_decode_byte((byte)Wire.read()); //
+      cnt++;
+    }
+    if (cnt > 5) {
+      lastZ = buttonZ;
+      lastC = buttonC;
+      lastJoyX = readJoyX();
+      lastJoyY = readJoyY();
+
+      cnt = 0;
+      joyX = (status[0]);
+      joyY = (status[1]);
+      for (i = 0; i < 3; i++)  
+        angles[i] = (status[i+2] << 2) + ((status[5] & (B00000011 << ((i+1)*2) ) >> ((i+1)*2))); 
+
+      buttonZ = !( status[5] & B00000001);
+      buttonC = !((status[5] & B00000010) >> 1);
+      _send_zero(); // send the request for next bytes
+      plugd = true;
+    }
+    else if (cnt == 0)
+      plugd = false;
+  }
+
+  float readAccelX() {
+    return (float)angles[0] - ZEROX;
+  }
+  float readAccelY() {
+    return (float)angles[1] - ZEROY;
+  }
+  float readAccelZ() {
+    return (float)angles[2] - ZEROZ;
+  }
+
+  boolean zPressed() {
+    return (buttonZ && ! lastZ);
+  }
+  boolean cPressed() {
+    return (buttonC && ! lastC);
+  }
+
+  // for using the joystick like a directional button
+  boolean rightJoy(int thresh=60) {
+    return (readJoyX() > thresh and lastJoyX <= thresh);
+  }
+
+  // for using the joystick like a directional button
+  boolean leftJoy(int thresh=60) {
+    return (readJoyX() < -thresh and lastJoyX >= -thresh);
+  }
+
+
+  int readJoyX() {
+    return (int) joyX - zeroJoyX;
+  }
+
+  int readJoyY() {
+    return (int)joyY - zeroJoyY;
+  }
+
+
+  // R, the radius, generally hovers around 210 (at least it does with mine)
+  int R() {
+    return sqrt(readAccelX() * readAccelX() +readAccelY() * readAccelY() + readAccelZ() * readAccelZ());  
+  }
+
+
+  // returns roll degrees
+  int readRoll() {
+    return (int)(atan2(readAccelX(),readAccelZ())/ M_PI * 180.0);
+  }
+
+  // returns pitch in degrees
+  int readPitch() {        
+    return (int) (acos(readAccelY()/RADIUS)/ M_PI * 180.0);  // optionally swap 'RADIUS' for 'R()'
+  }
+
+private:
+  byte _nunchuk_decode_byte (byte x)
+  {
+    x = (x ^ 0x17) + 0x17;
+    return x;
+  }
+
+  void _send_zero()
+  {
+    Wire.beginTransmission (0x52);	// transmit to device 0x52
+    Wire.write ((byte)0x00);		// sends one byte
+    Wire.endTransmission ();	// stop transmitting
+  }
+
+};
+
+
+#endif
+
+
+
+
+
+
+