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

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

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

/*ここまだ*/
const int LANCE_ANGLE_PARALLEL_1 =  90 -  53;  //度
const int LANCE_ANGLE_PARALLEL_2 =  90 -  43;  //度
const int LANCE_ANGLE_VERTICAL_L =  90 - 120;  //度
const int LANCE_ANGLE_VERTICAL_R =  90 -  57;  //度

const byte SPEED_DEFAULT = 128;

// Servo : DHCM (2013-03-13)
// 60deg : 1250 us
// 90deg : 1440 us
//120deg : 1630 us
const int STEERING_DEFAULT = 90; //度
const unsigned int STEERING_MIN = (1440-570); //us
const unsigned int STEERING_MAX = (1440+570); //us

const float STEERING_KP = 4.0;

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

const byte PIN_BUZZER =  3;
const byte PIN_ROT    =  2;

const byte PIN_SENSOR_0 = 12;  //右端
const byte PIN_SENSOR_1 = 13;
const byte PIN_SENSOR_2 = 14;
const byte PIN_SENSOR_3 = 15;
const byte PIN_SENSOR_4 = 16;
const byte PIN_SENSOR_5 = 17;
const byte PIN_SENSOR_6 = 18;
const byte PIN_SENSOR_7 = 19;  //左端

const byte PIN_SERVO_STEERING  =  9;
const byte PIN_SERVO_LANCE     = 10;

const byte PIN_MOTOR_LEFT  =  6;
const byte PIN_MOTOR_RIGHT =  5;

const byte MASK_LINE    = 0b01111110;
const byte MASK_MARKER  = 0b10000001;

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

Servo steeringServo;
Servo lanceServo;
Motor motorR;
Motor motorL;

//ラインが1、地面が0、LSBが左端、MSBが右端
byte sensor = 0x00;  

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

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

enum mode_t {
  MODE_STOP,      //停止
  MODE_STRAIGHT,  //まっすぐ進む
  MODE_TURN,      //左に曲がる
  MODE_ATTACK     //標的を狙う
} 
mode;

enum target_t {
  TARGET_NONE,
  TARGET_PARALLEL_1,  //平行標的1
  TARGET_PARALLEL_2,  //平行標的2
  TARGET_VERTICAL_R,  //右垂直標的
  TARGET_VERTICAL_L,  //左垂直標的
  TARGET_CYLINDER     //円筒標的
} 
target;

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

/* センサーを読んで、車体の動作を決定する */
void trace(){
  switch(sensor&MASK_LINE){
  case 0b01000000:
    steering(-5*STEERING_KP);
    motorL.speed(0);
    motorR.speed(SPEED_DEFAULT*0.7);
    break;
  case 0b01100000:
    steering(-4*STEERING_KP);
    motorL.speed(0);
    motorR.speed(SPEED_DEFAULT*0.7);
    break;
  case 0b00100000:
    steering(-3*STEERING_KP);
    motorL.speed(0);
    motorR.speed(SPEED_DEFAULT);
    break;
  case 0b00110000:
    steering(-2*STEERING_KP);
    motorL.speed(0);
    motorR.speed(SPEED_DEFAULT);
    break;
  case 0b00010000:
    steering(-1);
    motorL.speed(SPEED_DEFAULT);
    motorR.speed(SPEED_DEFAULT);
    break;
  case 0b00011000:
    steering(0);
    motorL.speed(SPEED_DEFAULT);
    motorR.speed(SPEED_DEFAULT);
    break;
  case 0b00001000:
    steering(1);
    motorL.speed(SPEED_DEFAULT);
    motorR.speed(SPEED_DEFAULT);
    break;
  case 0b00001100:
    steering(2*STEERING_KP);
    motorL.speed(SPEED_DEFAULT);
    motorR.speed(SPEED_DEFAULT);
    break;
  case 0b00000100:
    steering(3*STEERING_KP);
    motorL.speed(0);
    motorR.speed(0);
    break;
  case 0b00000110:
    steering(4*STEERING_KP);
    motorL.speed(0);
    motorR.speed(0);
    break;
  case 0b00000010:
    steering(5*STEERING_KP);
    motorL.speed(0);
    motorR.speed(0);
    break;
  }
}

/* ステアリングを中央からa[度]だけ動かす
 <- - 0 + ->
 |      |
 |      |
 []---{}---[]
 / .\
 */
void steering(int a){
  steeringServo.write(STEERING_DEFAULT - a);
}

/* ランスを中央からa[度]だけ動かす
 <- - 0 + ->
 / .\
 /  | \
 []+--{}--+[]
 */
void lance(int a){
  lanceServo.write(LANCE_DEFAULT - a);
}

/* センサーのアレイの初期化 */
void sensorInit(){
  pinMode(PIN_SENSOR_0, INPUT);  //右端
  pinMode(PIN_SENSOR_1, INPUT);
  pinMode(PIN_SENSOR_2, INPUT);
  pinMode(PIN_SENSOR_3, INPUT);
  pinMode(PIN_SENSOR_4, INPUT);
  pinMode(PIN_SENSOR_5, INPUT);
  pinMode(PIN_SENSOR_6, INPUT);
  pinMode(PIN_SENSOR_7, INPUT);  //左端
}

/* センサーアレイの状態を見る*/
byte sensorRead(){
  byte b;  //黒が1、白が0
  if(digitalRead(PIN_SENSOR_0)) b += 0b00000001; //右端
  if(digitalRead(PIN_SENSOR_1)) b += 0b00000010;
  if(digitalRead(PIN_SENSOR_2)) b += 0b00000100;
  if(digitalRead(PIN_SENSOR_3)) b += 0b00001000;
  if(digitalRead(PIN_SENSOR_4)) b += 0b00010000;
  if(digitalRead(PIN_SENSOR_5)) b += 0b00100000;
  if(digitalRead(PIN_SENSOR_6)) b += 0b01000000;
  if(digitalRead(PIN_SENSOR_7)) b += 0b10000000;  //左端
  //今回ラインが白なので、反転して、ラインを1とする
  b ^= 0xff;
  return sensor = b;
}

void modeSet(){
  if(distance > LAP_DISTANCE*5){  //5周したら停止
    mode = MODE_STOP;
    return;
  }
  switch(sensor&MASK_MARKER){
  default:
  case 0b00000000:
    mode = MODE_STRAIGHT;
    target = TARGET_NONE;
  case 0b10000001:
    modeChangedDistance = distance;
    mode = MODE_TURN;
    target = TARGET_NONE;
    break;
  case 0b00000001:
    modeChangedDistance = distance;
    mode = MODE_ATTACK;
    if(distance%LAP_DISTANCE < LAP_DISTANCE/7){  //まずは、E的をねらいたい
      target = TARGET_PARALLEL_1;
    }else if(distance%LAP_DISTANCE < LAP_DISTANCE/3){  //次に、F的をねらいたい
      target = TARGET_PARALLEL_2;
    }else{  //後半は垂直標的をねらいたい
      target = TARGET_VERTICAL_R;
    }
    break;
  case 0b10000000:
    modeChangedDistance = distance;
    mode = MODE_ATTACK;
    target = TARGET_VERTICAL_L;
    break;
  }
}

void stop(){
  motorL.mode(STOP);
  motorR.mode(STOP);
}

void straight(){
  trace();
  modeSet();
}

void turn(){
  trace();
  lance(0);
  if(distance - modeChangedDistance > TURN_DISTANCE){
    modeSet();
  }
}

void attack(){
  switch(target){
  default:
  case TARGET_NONE:
    trace();
    modeSet();
    break;
  case TARGET_PARALLEL_1:  //平行標的1
    trace();
    lance(LANCE_ANGLE_PARALLEL_1);
    if(distance - modeChangedDistance > 24){ //マーカーから少し進んで、叩く
      lance(LANCE_ANGLE_PARALLEL_1+8);
      modeSet();
    }
    break;
  case TARGET_PARALLEL_2:  //平行標的2
    trace();
    lance(LANCE_ANGLE_PARALLEL_2);
    if(distance - modeChangedDistance > 26){ //マーカーから少し進んで、叩く
      lance(LANCE_ANGLE_PARALLEL_2+8);
      modeSet();
    }
    break;
  case TARGET_VERTICAL_R:  //右垂直標
    trace();
    lance(LANCE_ANGLE_VERTICAL_R);
    modeSet();
    break;
  case TARGET_VERTICAL_L:  //左垂直標
    trace();
    lance(LANCE_ANGLE_VERTICAL_L);
    modeSet();
    break;
  case TARGET_CYLINDER:    //円筒標的
    trace();
    modeSet();
    break;
  }
}

void rot(){
  distance++;
}

void setup(){
  //サーボ
  steeringServo.attach(PIN_SERVO_STEERING, STEERING_MIN, STEERGING_MAX);
  lanceServo.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX);
  steering(0);
  lance(0);
  //モーター
  motorL.attach(PIN_MOTOR_LEFT);
  motorR.attach(PIN_MOTOR_RIGHT);
  motorL.mode(STOP);
  motorR.mode(STOP);
  //センサー
  sensorInit();
  //エンコーダー
  pinMode(PIN_ROT, INPUT);
  attachInterrupt(0, rot, FALLING);
  /* 3秒停止 */
  stop();
  delay(3000);
  /* スタート */
  modeSet();
}

void loop(){
  sensorRead();
  switch(mode){
  default:
  case MODE_STOP:      //停止
    stop();
    break;
  case MODE_STRAIGHT:  //まっすぐ進む
    straight();
    break;
  case MODE_TURN:      //左に曲がる
    turn();
    break;
  case MODE_ATTACK:    //標的を狙う
    attack();
    break;
  }
}