wiipper.ino

#include <Wiimote.h>
#include <HardwareSerial.h>
HardwareSerial oSerial(1); // RX, TX

typedef struct{
   int16_t steer;
   int16_t speed;
   uint32_t crc;
} Serialcommand;
Serialcommand oCmd;

int ny, nx;
boolean active = false;
boolean prev_c = false;
boolean connected = false;

void setup() {
  Serial.begin(115200);
  oSerial.begin(9600, SERIAL_8N1, 25, 26);
  //Wiimote::init();
  //Wiimote::register_callback(1, wiimote_callback);
}

void loop() {
  Send(100,0);
  //Wiimote::handle();
  //if (!connected) Serial.printf("Fuck!\n");
  
  //oSerial.printf("FUCK ME DADDY\n");
}

void hover(int raw_x, int raw_y, boolean raw_c, boolean raw_z) {
  if (raw_c){
    int min_x = 26;
    int max_x = 223;
    int dead_x = 124;
    int range_x = dead_x - min_x;
    
    int min_y = 32;
    int max_y = 226;
    int dead_y = 131;
    int range_y = dead_y - min_y;
    
    int deadzone = 5;
  
    int x = raw_x - dead_x;
    int y = raw_y - dead_y;
    boolean boost = raw_z;

    int16_t speed = 0;
    int16_t steer = 0;

    if (abs(x) < deadzone) {
      steer = 0;
    } else {
      steer = x;
      steer = (int16_t) (((float) steer) / range_x * 1000);
    }
    if (abs(y) < deadzone){
      speed = 0;
    } else {
      speed = y;
    };

    if (raw_z) {
      speed = (int16_t) (((float) speed) / range_y * 1000);
    }

    // Clamping
    speed = max((int16_t) -1000, min(speed, (int16_t) 1000));
    steer = max((int16_t) -1000, min(steer, (int16_t) 1000));

    
    Serial.printf("speed: %04d, steer: %04d\n", speed, steer);
    Send(speed, steer);
  } else {
    Send(0,0);
  }
}

void Send(int16_t iSpeed, int16_t steer)
{
  oCmd.steer = steer;
  oCmd.speed = iSpeed;

  uint32_t crc = 0;
  crc32((const void *)&oCmd, sizeof(Serialcommand)-4,   &crc);
  oCmd.crc = crc;
  
  oSerial.write((uint8_t *) &oCmd, sizeof(oCmd));
}

void wiimote_callback(uint8_t number, uint8_t* data, size_t len) {
  connected = true;
  //Serial.printf("wiimote number=%d len=%d ", number, len);
  if(data[1]==0x32){
    for (int i = 0; i < 4; i++) {
      //Serial.printf("%02X ", data[i]);
    }

    // http://wiibrew.org/wiki/Wiimote/Extension_Controllers/Nunchuck
    uint8_t* ext = data+4;
    /*Serial.printf(" ... Nunchuk: sx=%3d sy=%3d c=%d z=%d\n",
      ext[0],
      ext[1],
      0==(ext[5]&0x02),
      0==(ext[5]&0x01)
    );*/
    hover(ext[0], ext[1], 0==(ext[5]&0x02), 0==(ext[5]&0x01));
  }else{
    for (int i = 0; i < len; i++) {
      Serial.printf("%02X ", data[i]);
    }
    Serial.print("\n");
  }
}

uint32_t crc32_for_byte(uint32_t r) 
{
  for(int j = 0; j < 8; ++j)
    r = (r & 1? 0: (uint32_t)0xEDB88320L) ^ r >> 1;
  return r ^ (uint32_t)0xFF000000L;
}

void crc32(const void *data, size_t n_bytes, uint32_t* crc) {
  static uint32_t table[0x100];
  if(!*table)
    for(size_t i = 0; i < 0x100; ++i)
      table[i] = crc32_for_byte(i);
  for(size_t i = 0; i < n_bytes; ++i)
    *crc = table[(uint8_t)*crc ^ ((uint8_t*)data)[i]] ^ *crc >> 8;
}