/*
 * NunchuckServo
 *
 * 2007 Tod E. Kurt, http://todbot.com/blog/
 *
 * Код для считывания Wii-Нунчака взят из Windmeadow Labs
 *   http://www.windmeadow.com/node/42
 */

#include <Wire.h>

int ledPin = 13;
int servoPin = 7;      // Управляющий пин для сервомотора

int pulseWidth = 0;    // значение импульса для сервы
int refreshTime = 20;  // время в миллисекундах, необходимое между импульсами
long lastPulse;
int minPulse = 700;   // минимальная ширина импульса
int loop_cnt=0;

void setup()
{
  Serial.begin(19200);
  pinMode(servoPin, OUTPUT);  // объявить пин сервы выходом
  pulseWidth = minPulse;      // установить положение мотора на минимум

  nunchuck_init(); // инициализация соединения
  Serial.print("NunchuckServo ready\n");
}

void loop()
{
  checkNunchuck();
  updateServo();   // обновить позицию сервы

  if( nunchuck_zbutton() )      // включить светодиод, если нажата кнопка z
    digitalWrite(ledPin, HIGH);
  else 
    digitalWrite(ledPin,LOW);

  delay(1);        // это для того, чтобы знать время на итерацию цикла
}


void checkNunchuck()
{
  if( loop_cnt > 100 ) {  // loop() вызывается раз в 1мс, значит, эта - раз в 100 мс
    
    nunchuck_get_data();
    nunchuck_print_data();

    float tilt = nunchuck_accelx();    // ось x, в диапазоне ~70 - ~185
    tilt = (tilt - 70) * 1.5;        // перевести в угол в градусах, грубо  
    pulseWidth = (tilt * 9) + minPulse; // перевести угол в микросекунды
    
    loop_cnt = 0;  // перезапустить цикл
  }
  loop_cnt++; 
  
}

// вызывается каждый раз в loop().
// используются глобальные переменные servoPin, pulsewidth, lastPulse, и refreshTime
void updateServo()
{
  // снова послать импульс на серву, если время перерыва (20 миллисекунд) прошло:
  if (millis() - lastPulse >= refreshTime) {
    digitalWrite(servoPin, HIGH);   // Включить мотор
    delayMicroseconds(pulseWidth);  // Длина импульса определяет положение мотора
    digitalWrite(servoPin, LOW);    // Выключить мотор
    lastPulse = millis();           // сохранить время последнего импульса
  }
}




//
// Функции Нунчака
//

static uint8_t nunchuck_buf[6];   // массив для хранения данных нунчака,
 
// запустить систему I2C, подсоединиться к шине I2C,
// и сообщить нунчаку, что мы с ним разговариваем
void nunchuck_init()
{ 
  Wire.begin();	                // подключиться к i2c как ведущий
  Wire.beginTransmission(0x52);	// передать на устройство 0x52
  Wire.send(0x40);		// передать адрес в памяти
  Wire.send(0x00);		// передать нуль
  Wire.endTransmission();	// остановить передачу
}

// Посылает нунчаку запрос о данных
// было "send_zero()"
void nunchuck_send_request()
{
  Wire.beginTransmission(0x52);	// передать на устройство 0x52
  Wire.send(0x00);		// передать один байт
  Wire.endTransmission();	// остановить передачу
}

// Получить данные от нунчака, 
int nunchuck_get_data()
{
    int cnt=0;
    Wire.requestFrom (0x52, 6);	// запросить данные у нунчака
    while (Wire.available ()) {
      // получить байт как целое число
      nunchuck_buf[cnt] = nunchuk_decode_byte(Wire.receive());
      cnt++;
    }
    nunchuck_send_request();  // послать запрос о следующей
			      // порции данных
    // Если мы получили 6 байтов, тогда напечатаем их
    if (cnt >= 5) {
     return 1;   // успех
    }
    return 0; //неудача
}

// Печатаем полученные данные
// Акселерометр выдаёт 10 бит
// Так что мы считываем 8 битов, затем нужно добавить
// последние 2 бита.  Поэтому я
// умножаю их на 2 * 2
void nunchuck_print_data()
{ 
  static int i=0;
  int joy_x_axis = nunchuck_buf[0];
  int joy_y_axis = nunchuck_buf[1];
  int accel_x_axis = nunchuck_buf[2]; // * 2 * 2; 
  int accel_y_axis = nunchuck_buf[3]; // * 2 * 2;
  int accel_z_axis = nunchuck_buf[4]; // * 2 * 2;

  int z_button = 0;
  int c_button = 0;

  // байт nunchuck_buf[5] содержит биты для кнопок z и c
  // также содержит младшие биты для с данными акселерометра
  // так что посмотрим каждый бит байта outbuf[5]
  if ((nunchuck_buf[5] >> 0) & 1) 
    z_button = 1;
  if ((nunchuck_buf[5] >> 1) & 1)
    c_button = 1;

  if ((nunchuck_buf[5] >> 2) & 1) 
    accel_x_axis += 2;
  if ((nunchuck_buf[5] >> 3) & 1)
    accel_x_axis += 1;

  if ((nunchuck_buf[5] >> 4) & 1)
    accel_y_axis += 2;
  if ((nunchuck_buf[5] >> 5) & 1)
    accel_y_axis += 1;

  if ((nunchuck_buf[5] >> 6) & 1)
    accel_z_axis += 2;
  if ((nunchuck_buf[5] >> 7) & 1)
    accel_z_axis += 1;

  Serial.print(i,DEC);
  Serial.print("\t");
  
  Serial.print("joy:");
  Serial.print(joy_x_axis,DEC);
  Serial.print(",");
  Serial.print(joy_y_axis, DEC);
  Serial.print("  \t");

  Serial.print("acc:");
  Serial.print(accel_x_axis, DEC);
  Serial.print(",");
  Serial.print(accel_y_axis, DEC);
  Serial.print(",");
  Serial.print(accel_z_axis, DEC);
  Serial.print("\t");

  Serial.print("but:");
  Serial.print(z_button, DEC);
  Serial.print(",");
  Serial.print(c_button, DEC);

  Serial.print("\r\n");  // новая срока
  i++;
}

// Перекодировать данные в формат, который принимают
// большинство драйверов wiimote. Нужно только, если 
// используете один из стандартных драйверов wiimote
char nunchuk_decode_byte (char x)
{
  x = (x ^ 0x17) + 0x17;
  return x;
}

// возвращает состояние кнопки z: 1=нажата, 0=не нажата
int nunchuck_zbutton()
{
   return ((nunchuck_buf[5] >> 0) & 1) ? 0 : 1;  // магия вуду
}

// возвращает состояние кнопки c: 1=нажата, 0=не нажата
int nunchuck_cbutton()
{
   return ((nunchuck_buf[5] >> 1) & 1) ? 0 : 1;  // магия вуду
}

// возвращает показания оси джойстика x
int nunchuck_joyx()
{
  return nunchuck_buf[0]; 
}

// возвращает показания оси джойстика y
int nunchuck_joyy()
{
  return nunchuck_buf[1];
}

// возвращает показания акселерометра по оси x
int nunchuck_accelx()
{
  return nunchuck_buf[2];   // ИСПРАВИТЬ: здесь выкидываются 2 бита данных
}

// возвращает показания акселерометра по оси y
int nunchuck_accely()
{
  return nunchuck_buf[3];   // ИСПРАВИТЬ: здесь выкидываются 2 бита данных
}

// возвращает показания акселерометра по оси z
int nunchuck_accelz()
{
  return nunchuck_buf[4];   // ИСПРАВИТЬ: здесь выкидываются 2 бита данных
}