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

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

/* 標的の直前にあるマーカーを見つけ出す範囲 : {最小, 最大} */
struct range_t{
  unsigned long min;
  unsigned long max;
};

/* 標的のためのランスの角度 : {before[度], after[度]} */
struct lanceAngle_t{
  int before;
  int after;
};

/* 標的の情報 */
struct target_t{
  struct range_t range;
  unsigned long way;  //マーカーから標的までの距離
  struct lanceAngle_t lanceAngle;
};

/**************************************/
/* 動作パラメーター */

unsigned long lapDistance  = 28550; //ロータリーエンコーダーのカウント数/周
unsigned long turnDistance = 4650;  //カーブ中でのカウント数

struct target_t targetA = {{lapDistance/2,    lapDistance*0.6 }, 900, { 29,  0}};
struct target_t targetB = {{lapDistance/2,    lapDistance*0.75}, 900, {-32,  0}};
struct target_t targetC = {{lapDistance*0.6,  lapDistance*0.85}, 900, { 34,  0}};
struct target_t targetD = {{lapDistance*0.75, lapDistance*0.85}, 900, {-42,  0}};
struct target_t targetE = {{ 500,4000}, 750, { 48, 53}};
struct target_t targetF = {{4000,7000}, 750, { 60, 67}};

int laps = 3;  //最大周回数
bool silent = false;  //サイレントモード(true:ブザーを鳴らさない)

int steeringAngleMin = -30; //度
int steeringAngleMax =  10; //度

int deltaSpeedMax = 500;  //最大速度差
int speedMax = 255;  //最大速度

int cycle = 1;  //動作周波数(ms)

int errorCountThreshold = 500;  //最大読み取りエラー回数(これを超えると停止する)

/**************************************/
/* ピンの設定 */

// Servo : SC-1267SG
unsigned int steeringServoCenter = 1808;  //us
unsigned int steeringServoMin = (steeringServoCenter-780); //us
unsigned int steeringServoMax = (steeringServoCenter+780); //us

// Servo : SC-0352
unsigned int lanceServoCenter = 1560;  //us
unsigned int lanceServoMin = (lanceServoCenter-780); //us
unsigned int lanceServoMax = (lanceServoCenter+780); //us

unsigned char buzzerPin  = 3;
unsigned char rotPin     = 2;

unsigned char numSensors = 8;
unsigned char sensorPin[] = {19,18,17,16,15,14,13,12};  //右端から順番に左端へ

unsigned char lineSensorMask    = 0b01111110;
unsigned char markerSensorMask  = 0b10000001;

unsigned char steeringServoPin  =  9;
unsigned char lanceServoPin     = 10;

unsigned char motorLeftEnablePin     = 5;
unsigned char motorLeftDirectionPin  = 4;
unsigned char motorRightEnablePin    = 6;
unsigned char motorRightDirectionPin = 7;

/**************************************/
/* グローバル変数 */

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

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

// 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
int deltaSpeed = 0;
// 駆動用モーターの速度
int speed = 0;

// ステアリングの角度
long angle = 0; // 度

int errorCount = 0;

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

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

/* 動作モード */
enum mode_t {
  MODE_STOP,      //停止
  MODE_STRAIGHT,  //まっすぐ進む
  MODE_TURN,      //左に曲がる
  MODE_ATTACK_E,  //平行標的1
  MODE_ATTACK_F,  //平行標的2
  MODE_ATTACK_A,  //右垂直標的
  MODE_ATTACK_B,  //左垂直標的
  MODE_ATTACK_C,  //右垂直標的
  MODE_ATTACK_D,  //左垂直標的
  MODE_ATTACK_CYLINDER     //円筒標的
} mode;

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

/* センサーを読んで、車体の動作を決定する */
void trace(){
  switch(sensor&lineSensorMask){
  default:
    errorCount++;
    return;
  case 0b01000000:
    speed = speedMax*0.3;
    deltaSpeed = -145;
    angle = -15;
    break;
  case 0b01100000:
    speed = speedMax*0.5;
    deltaSpeed = -115;
    angle = -13;
    break;
  case 0b00100000:
    speed = speedMax*0.7;
    deltaSpeed = -95;
    angle = -11;
    break;
  case 0b00110000:
    speed = speedMax*0.9;
    deltaSpeed = -75;
    angle = -9;
    break;
  case 0b00010000:
    speed = speedMax;
    deltaSpeed = 0;
    angle = -1;
    break;
  case 0b00011000:
    speed = speedMax;
    deltaSpeed = 0;
    angle = 0;
    break;
  case 0b00001000:
    speed = speedMax;
    deltaSpeed = 0;
    angle = 1;
    break;
  case 0b00001100:
    speed = speedMax;
    deltaSpeed = 0;
    angle = 2;
    break;
  case 0b00000100:
    speed = speedMax;
    deltaSpeed = 3;
    angle = 3;
    break;
  case 0b00000110:
    speed = speedMax;
    deltaSpeed = 4;
    angle = 4;
    break;
  case 0b00000010:
    speed = 0;
    deltaSpeed = 5;
    angle = 5;
    break;
  }
  errorCount = 0;
}

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

void setup(){
  //サーボ
  steeringServo.attach(steeringServoPin, steeringServoMin, steeringServoMax);
  lanceServo.attach(lanceServoPin, lanceServoMin, lanceServoMax);
  steering(0);
  lance(0);
  //駆動用モーター
  motorL.attach(motorLeftEnablePin,  motorLeftDirectionPin);
  motorR.attach(motorRightEnablePin, motorRightDirectionPin);
  motorL.mode(STOP);
  motorR.mode(STOP);
  //センサー
  sensorInit();
  //エンコーダー
  pinMode(rotPin, INPUT);
  attachInterrupt(0, rot, RISING);
  //ブザー
  startBeep();
  /* 3秒停止 */
  delay(3000);
  /* スタート */
  mode = MODE_STRAIGHT;
  MsTimer2::set(cycle, run);
  MsTimer2::start();
}

void loop(){
  scanSensor();
}

/* 車体の動作を決定する */
void run(){
  if(distance > lapDistance*laps || errorCount > errorCountThreshold){
    mode = MODE_STOP;
  }
  switch(mode){
  default:
  case MODE_STOP:      //停止
    MsTimer2::stop();
    lanceServo.detach();
    steeringServo.detach();
    motorL.mode(STOP);
    motorR.mode(STOP);
    while(1){
      ;
    }
    break;
  case MODE_STRAIGHT:  //まっすぐ進む
    trace();
    modeSet();
    break;
  case MODE_TURN:      //左に曲がる
    trace();
    lance(0);
    if(distance - modeChangedDistance > turnDistance){
      mode = MODE_STRAIGHT;
    }
    break;
  case MODE_ATTACK_E:  //平行標的1
    trace();
    lance(targetE.lanceAngle.before);
    if(distance - modeChangedDistance > targetE.way){ //マーカーから少し進んで、叩く
      lance(targetE.lanceAngle.after);
      mode = MODE_STRAIGHT;
    }
    break;
  case MODE_ATTACK_F:  //平行標的2
    trace();
    lance(targetF.lanceAngle.before);
    if(distance - modeChangedDistance > targetF.way){ //マーカーから少し進んで、叩く
      lance(targetF.lanceAngle.after);
      mode = MODE_STRAIGHT;
    }
    break;
  case MODE_ATTACK_A:  //右垂直標
    trace();
    lance(targetA.lanceAngle.before);
    if(distance - modeChangedDistance > targetA.way){ //マーカーから少し進んで、まっすぐに戻す
      lance(targetA.lanceAngle.after);
      mode = MODE_STRAIGHT;
    }
    break;
  case MODE_ATTACK_C:  //右垂直標
    trace();
    lance(targetC.lanceAngle.before);
    if(distance - modeChangedDistance > targetC.way){ //マーカーから少し進んで、まっすぐに戻す
      lance(targetC.lanceAngle.after);
      mode = MODE_STRAIGHT;
    }
    break;
  case MODE_ATTACK_B:  //左垂直標
    trace();
    lance(targetB.lanceAngle.before);
    if(distance - modeChangedDistance > targetB.way){ //マーカーから少し進んで、まっすぐに戻す
      lance(targetB.lanceAngle.after);
      mode = MODE_STRAIGHT;
    }
    break;
  case MODE_ATTACK_D:  //左垂直標
    trace();
    lance(targetD.lanceAngle.before);
    if(distance - modeChangedDistance > targetD.way){ //マーカーから少し進んで、まっすぐに戻す
      lance(targetD.lanceAngle.after);
      mode = MODE_STRAIGHT;
    }
    break;
  case MODE_ATTACK_CYLINDER:    //円筒標的
    mode = MODE_STRAIGHT;
    break;
  }
  
  /* 駆動モーター用 */
  deltaSpeed = constrain(deltaSpeed, -deltaSpeedMax, deltaSpeedMax);
  if(deltaSpeed<0){
    motorL.speed(speed + deltaSpeed);
    motorR.speed(speed);
  }else{
    motorL.speed(speed);
    motorR.speed(speed - deltaSpeed);
  }
  
  /* ステアリングサーボ用 */
  angle = constrain(angle, steeringAngleMin, steeringAngleMax);
  steering(angle);
}

/* モードを決定する */
void modeSet(){
  unsigned long way = distance%lapDistance;
  switch(sensor&markerSensorMask){
  default:
  case 0b00000000:
    mode = MODE_STRAIGHT;
    break;
  case 0b10000001:
    modeChangedDistance = distance;
    mode = MODE_TURN;
    break;
  case 0b00000001: //E,F,A,C
    modeChangedDistance = distance;
    if(targetE.range.min <= way && way < targetE.range.max) mode = MODE_ATTACK_E;
    if(targetF.range.min <= way && way < targetF.range.max) mode = MODE_ATTACK_F;
    if(targetA.range.min <= way && way < targetA.range.max) mode = MODE_ATTACK_A;
    if(targetC.range.min <= way && way < targetC.range.max) mode = MODE_ATTACK_C;
    break;
  case 0b10000000: //B,D
    modeChangedDistance = distance;
    if(targetB.range.min <= way && way < targetB.range.max) mode = MODE_ATTACK_B;
    if(targetD.range.min <= way && way < targetD.range.max) mode = MODE_ATTACK_D;
    break;
  }
}