// IDE 0022
//-------------
// nicoo
// april 2013
//-------------

int useless_variable_to_force_ide_to_insert_its_lines_here = 0;

#include <Servo.h>

// servo 0: left arm servo
// servo 1: right arm servo
// servo 2: claw
// servo 3: claw arm
// servo 4:
// servo 5:

#define NUM_SERVO 4
const int SERVO_PIN[NUM_SERVO] = { 11, 10, 9, 6 };
const int SERVO_MIN[NUM_SERVO] = { 850, 1150, 1500, 900 };
const int SERVO_MAX[NUM_SERVO] = { 1900, 2200, 2100, 2100 };
Servo servo[NUM_SERVO];
int servo_microsec[NUM_SERVO] = { 1500, 1500, 1800, 1500 };

//-- Nunchuk ---------------------
#include <Wire.h>
#include <ArduinoNunchuk.h>
#include <NicoArduinoNunchuk.h>
NicoArduinoNunchuk nunchuk;
//----------

void setup()
{
  int index;
  for(index=0; index < NUM_SERVO; index++)
    {
    servo[index].attach(SERVO_PIN[index], SERVO_MIN[index], SERVO_MAX[index]);
    pinMode(SERVO_PIN[index], OUTPUT);
    }
  nunchuk.init();

  Serial.begin(115200); // debug

  // visual check that all servos + nunchuk are operationnal.
  for(index=0; index < NUM_SERVO; index++)
    servo[index].writeMicroseconds(0.5*(SERVO_MIN[index]+SERVO_MAX[index]));
  while (true)
    {
    nunchuk.update();
    if (nunchuk.cPressed) 
      break;
    }
  // press 'c' button on Nunchuk to end visual check
}

int claw_open = 1;
int arm_up = 0;
int claw_out = 0;

void loop()
{
  nunchuk.update();
  nunchuk.prt(Serial);

  // arm control
  if (nunchuk.cPressed) arm_up = !arm_up;
  int arm_pos = nunchuk.degreeY();

  if (arm_up)
    {
    if (arm_pos > 90) arm_pos = 90;
    servo_microsec[0] = map(arm_pos, 0, 90, SERVO_MAX[0], SERVO_MIN[0]);
    servo_microsec[1] = map(arm_pos, 0, 90, SERVO_MIN[1], SERVO_MAX[1]);
    }
  else
    {
    if (arm_pos < 90) arm_pos = 90;
    servo_microsec[0] = map(arm_pos, 90, 180, SERVO_MAX[0], SERVO_MIN[0]);
    servo_microsec[1] = map(arm_pos, 90, 180, SERVO_MIN[1], SERVO_MAX[1]);
    }
  // end arm control
  
  // claw
  int claw_pos = nunchuk.degreeX();

  if (!claw_out && nunchuk.right)
    claw_out = 1;
  if (claw_out && nunchuk.left)
    claw_out = 0;

  if (!claw_out)
    {
    servo_microsec[3] = SERVO_MAX[3];
    }
  else
    {
    if (claw_pos < 90) claw_pos = 90;
    servo_microsec[3] = map(claw_pos, 90, 180, 0.5*(SERVO_MIN[3]+SERVO_MAX[3]), SERVO_MIN[3]);
    }

  if (nunchuk.zPressed) claw_open = !claw_open;
  servo_microsec[2] = claw_open ? SERVO_MAX[2] : SERVO_MIN[2];
  // end claw
  
  int index;
  for(index=0; index < NUM_SERVO; index++)
    servo[index].writeMicroseconds(servo_microsec[index]);
  delay(10);
}