Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <math.h>
- #include "Wire.h"
- #include "WiiChuck.h"
- #define MOTION_STOP (0 << 4)
- #define MOTION_FWD (1 << 4)
- #define MOTION_REV (2 << 4)
- #define ANGLE_STRAIGHT (0 << 2)
- #define ANGLE_LEFT (1 << 2)
- #define ANGLE_RIGHT (2 << 2)
- #define BUTTON_C (1 << 1)
- #define BUTTON_Z (1 << 0)
- #define HALF_PI (3.1415 / 2)
- WiiChuck chuck = WiiChuck();
- void setup() {
- Serial.begin(19200);
- chuck.begin();
- chuck.update();
- }
- int i;
- void loop() {
- delay(50);
- chuck.update();
- int x = chuck.readJoyX();
- int y = chuck.readJoyY();
- uint8_t magnitude = sqrt(sq(x) + sq(y));
- if (magnitude > 100)
- magnitude = 100;
- uint8_t angle = ( 100 * (float) atan((float)abs(x)/(float)abs(y)) / HALF_PI );
- uint8_t flags;
- if (y >= 10)
- flags |= MOTION_FWD;
- else if (y <= -10)
- flags |= MOTION_REV;
- else
- flags |= MOTION_STOP;
- if (x >= 10)
- flags |= ANGLE_RIGHT;
- else if (x <= -10)
- flags |= ANGLE_LEFT;
- else
- flags |= ANGLE_STRAIGHT;
- if (chuck.buttonC)
- flags |= BUTTON_C;
- if (chuck.buttonZ)
- flags |= BUTTON_Z;
- // curly braces have an ascii val > 100, all of the other data is <= 100
- uint8_t packet[6];
- packet[0] = '{';
- packet[1] = magnitude;
- packet[2] = angle;
- packet[3] = flags;
- packet[4] = (magnitude ^ angle ^ flags) & 0b00111111; // checksum
- packet[5] = '}';
- Serial.write(packet, 6);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement