/*
競技用ランサーロボット 源内
(C)2013 kou029w - MIT License
*/

#include <Tone.h>
#include <Servo.h>
#include <QTRSensors.h>
#include "Motor.h"

const unsigned long LAP_DISTANCE  = 500;  //ロータリーエンコーダーのカウント数/周
const unsigned long TURN_DISTANCE = 1500;  //カーブ中でのカウント数

const int LANCE_ANGLE_PARALLEL_1 =  90 -  62;  //度
const int LANCE_ANGLE_PARALLEL_2 =  90 -  50;  //度
const int LANCE_ANGLE_VERTICAL_L =  90 - 120;  //度
const int LANCE_ANGLE_VERTICAL_R =  90 -  62;  //度

const unsigned char SPEED_DEFAULT = 100;

//Servo  : SC-0352
// 60deg : 1265 us
// 90deg : 1535 us
//120deg : 1805 us
const int LANCE_DEFAULT = 90; //度
const unsigned int LANCE_MIN = (1620-780); //us
const unsigned int LANCE_MAX = (1620+780); //us

const unsigned char PIN_BUZZER =  3;
const unsigned char PIN_ROT    =  2;

const unsigned char PIN_SERVO_LANCE = 9;

const unsigned char NUM_SENSORS = 2;
unsigned char PIN_SENSORS[NUM_SENSORS] = {13, 12};  // 左端...右端

const unsigned char PIN_MOTOR_LEFT  =  6;
const unsigned char PIN_MOTOR_RIGHT =  5;

/**************************************/

Servo lanceServo;
Motor motorL;
Motor motorR;

QTRSensorsRC sensor(PIN_SENSORS, NUM_SENSORS);
unsigned int sensorValues[NUM_SENSORS];

volatile unsigned long distance = 0;
unsigned long modeChangedDistance = 0;

int error_count = 0;

/**************************************/

enum mode_t {
  MODE_STOP,      //停止
  MODE_STRAIGHT,  //まっすぐ進む
  MODE_TURN,      //左に曲がる
  MODE_ATTACK_PARALLEL_1,  //平行標的1
  MODE_ATTACK_PARALLEL_2,  //平行標的2
  MODE_ATTACK_VERTICAL_R,  //右垂直標的
  MODE_ATTACK_VERTICAL_L,  //左垂直標的
  MODE_ATTACK_CYLINDER     //円筒標的
} 
mode;

/**************************************/


/* センサーを読んで、車体の動作を決定する */
void trace(){
  error_count++;
  for(int i=0; i<NUM_SENSORS; i++){
    if(sensorValues[i] < 600) error_count = 0;
  }

  if(sensorValues[0] > 600) motorR.speed(SPEED_DEFAULT*0.6);
  else motorR.speed(SPEED_DEFAULT);
  if(sensorValues[1] > 600) motorL.speed(SPEED_DEFAULT*0.6);
  else motorL.speed(SPEED_DEFAULT);
}

/* ランスを中央からa[度]だけ動かす
  <- - 0 + ->
     / .\
    /  | \
 []+--{}--+[]
*/
void lance(int a){
  lanceServo.write(LANCE_DEFAULT - a);
}
void modeSet(){
  if(distance > LAP_DISTANCE || error_count > 100){
    mode = MODE_STOP;
    return;
  }
}

void rot(){
  distance++;
}

void startBeep(){
  tone(PIN_BUZZER,2000);
  delay(100);
  tone(PIN_BUZZER,1000);
  delay(100);
  noTone(PIN_BUZZER);
}
/**************************************/

void setup(){
  Serial.begin(9600);
  //サーボ
  lanceServo.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX);
  lance(0);
  //モーター
  motorL.attach(PIN_MOTOR_LEFT);
  motorR.attach(PIN_MOTOR_RIGHT);
  motorL.mode(STOP);
  motorR.mode(STOP);
  //エンコーダー
  pinMode(PIN_ROT, INPUT);
  attachInterrupt(0, rot, RISING);
  //ブザー
  pinMode(PIN_BUZZER, OUTPUT);
  startBeep();
  /* 3秒停止 */
  motorL.mode(STOP);
  motorR.mode(STOP);
  delay(3000);
  /* スタート */
  mode = MODE_STRAIGHT;
}

void loop(){
  Serial.println(distance);
  sensor.read(sensorValues);
  switch(mode){
  case MODE_STOP:      //停止
    motorL.mode(STOP);
    motorR.mode(STOP);
    tone(PIN_BUZZER, 2000);
    delay(2000);
    noTone(PIN_BUZZER);
    while(1){
      ;
    };
  case MODE_STRAIGHT:  //まっすぐ進む
    trace();
    modeSet();
    break;
  }
}