SHARE
TWEET

Untitled

a guest Sep 13th, 2017 47 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <math.h>
  2.  
  3. #include "Wire.h"
  4. #include "WiiChuck.h"
  5.  
  6. #define MOTION_STOP     (0 << 4)
  7. #define MOTION_FWD      (1 << 4)
  8. #define MOTION_REV      (2 << 4)
  9.  
  10. #define ANGLE_STRAIGHT  (0 << 2)
  11. #define ANGLE_LEFT      (1 << 2)
  12. #define ANGLE_RIGHT     (2 << 2)
  13.  
  14. #define BUTTON_C        (1 << 1)
  15. #define BUTTON_Z        (1 << 0)
  16.  
  17. #define HALF_PI (3.1415 / 2)
  18.  
  19. WiiChuck chuck = WiiChuck();
  20.  
  21. void setup() {
  22.   Serial.begin(19200);
  23.   chuck.begin();
  24.   chuck.update();
  25. }
  26.  
  27. int i;
  28.  
  29. void loop() {
  30.   delay(50);
  31.   chuck.update();
  32.  
  33.   int x = chuck.readJoyX();
  34.   int y = chuck.readJoyY();
  35.    
  36.   uint8_t magnitude = sqrt(sq(x) + sq(y));
  37.   if (magnitude > 100)
  38.     magnitude = 100;
  39.    
  40.   uint8_t angle = ( 100 * (float) atan((float)abs(x)/(float)abs(y)) / HALF_PI );
  41.  
  42.   uint8_t flags;
  43.  
  44.   if (y >= 10)
  45.     flags |= MOTION_FWD;
  46.   else if (y <= -10)
  47.     flags |= MOTION_REV;
  48.   else
  49.     flags |= MOTION_STOP;
  50.    
  51.   if (x >= 10)
  52.     flags |= ANGLE_RIGHT;
  53.   else if (x <= -10)
  54.     flags |= ANGLE_LEFT;
  55.   else
  56.     flags |= ANGLE_STRAIGHT;
  57.    
  58.   if (chuck.buttonC)
  59.     flags |= BUTTON_C;
  60.   if (chuck.buttonZ)
  61.     flags |= BUTTON_Z;
  62.  
  63.   // curly braces have an ascii val > 100, all of the other data is <= 100
  64.   uint8_t packet[6];
  65.   packet[0] = '{';
  66.   packet[1] = magnitude;
  67.   packet[2] = angle;
  68.   packet[3] = flags;
  69.   packet[4] = (magnitude ^ angle ^ flags) & 0b00111111; // checksum
  70.   packet[5] = '}';
  71.  
  72.   Serial.write(packet, 6);
  73. }
RAW Paste Data
Pastebin PRO Autumn Special!
Get 40% OFF on Pastebin PRO accounts!
Top