/*
競技用ランサーロボット
半蔵 2.1
(C)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt]
*/

/*
    <前>

[7 654321 0]
     |
     |
[]---*----[]
     | /
     |/
[]---#----[]
*/

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

/**************************************/
//#define LINE_BLACK //白地・黒ライン有効
#define PIN_SENSOR ( ((PINC<<2) & ~0x03) | ((PINB>>4) & 0x03) )

const unsigned int TARGET_NONE_COUNT = 85;

const int LANCE_ANGLE0 =   0; //度
const int LANCE_ANGLE1 =  43;
const int LANCE_ANGLE2 =  54;
const int LANCE_ANGLE3 = -33;
const int LANCE_ANGLE4 =  34;
const unsigned int LANCE_INTERVAL = 70; //ms

const byte SPEED_DEFAULT = 0xff;

const int HANDLE_DEFAULT = 90; //度
const unsigned int HANDLE_MIN = ( 800+30); //ms
const unsigned int HANDLE_MAX = (2300+30); //ms

const int LANCE_DEFAULT = 90; //度
const unsigned int LANCE_MIN =  544; //ms
const unsigned int LANCE_MAX = 2400; //ms

const byte PIN_LED           =  4;
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_HANDLE  = 10;
const byte PIN_SERVO_LANCE   =  9;

const byte PIN_MOTOR_LEFT_1  =  5;
const byte PIN_MOTOR_LEFT_2  =  4;
const byte PIN_MOTOR_RIGHT_1 =  6;
const byte PIN_MOTOR_RIGHT_2 =  7;

const byte MODE_STOP  =  0;
const byte MODE_TRACE = 10;
const byte MODE_RIGHT = 21; // 右カーブ
const byte MODE_LEFT  = 22; // 左カーブ

const byte TARGET_NONE     = 40;
const byte TARGET_PARALLEL = 41;
const byte TARGET_VERTICAL = 42;
const byte TARGET_CYLINDER = 43;

const byte MASK_MODE_TRACE         = 0b01111110;
const byte MASK_CHECK_MARKER       = 0b10000001;
const byte MASK_CHECK_MARKER_RIGHT = 0b00000001;
const byte MASK_CHECK_MARKER_LEFT  = 0b10000000;

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

Servo servoHandle;
Servo servoLance;
Motor motorR;
Motor motorL;

byte mode;
byte target;
unsigned int counterOld;
char handleAngle;
char lanceAngle;

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

void trace(byte sensor){
  switch( sensor & MASK_MODE_TRACE ){
  case 0b01000000:
    handleAngle = -20;
    motorMode(GO, GO, 0x40, 0xff);
    lanceAngle = 0;
    break;
  case 0b01100000:
    handleAngle = -16;
    motorMode(GO, GO, 0xff, 0xff);
    lanceAngle = 0;
    break;
  case 0b00100000:
    handleAngle = -12;
    motorMode(GO, GO, 0xff, 0xff);
    lanceAngle = 0;
    break;
  case 0b00110000:
    handleAngle = -5;
    motorMode(GO, GO, 0xff, 0xff);
    break;
  case 0b00010000:
    handleAngle = -2;
    motorMode(GO, GO, 0xff, 0xff);
    break;
  case 0b00011000:
    handleAngle = 0;
    motorMode(GO, GO, 0xff, 0xff);
    break;
  case 0b00001000:
    handleAngle = 2;
    motorMode(GO, GO, 0xff, 0xff);
    break;
  case 0b00001100:
    handleAngle = 5;
    motorMode(GO, GO, 0xff, 0xff);
    break;
  case 0b00000100:
    handleAngle = 7;
    motorMode(GO, GO, 0xff, 0xff);
    break;
  case 0b00000110:
    handleAngle = 10;
    motorMode(GO, GO, 0xff, 0xff);
    break;
  case 0b00000010:
    handleAngle = 12;
    motorMode(GO, GO, 0xff, 0xff);
    break;
  }
}

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

void motorMode(byte motorLeftMode, byte motorRightMode, byte motorLeftSpeed, byte motorRightSpeed){
  motorL.mode( motorLeftMode,  motorLeftSpeed);
  motorR.mode(motorRightMode, motorRightSpeed);
}

void handle(char handle_angle){
  servoHandle.write(HANDLE_DEFAULT + handle_angle);
}

void lance(char lance_angle){
  servoLance.write(LANCE_DEFAULT - lance_angle);
}

/* ロータリーエンコーダーの変化を見る */
volatile unsigned long counter = 0;
void count(){
  static byte rot[2] = {0};
  rot[1] = digitalRead(PIN_ROT);
  if(rot[1] != rot[0]){
    counter++;
    rot[0] = rot[1];
  }
}

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);
}

void setup() {
  //サーボ
  servoHandle.attach(PIN_SERVO_HANDLE, HANDLE_MIN, HANDLE_MAX);
  servoLance.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX);
  handle(0);
  lance(0);
  //モーター
  motorL.attach( PIN_MOTOR_LEFT_1,  PIN_MOTOR_LEFT_2);
  motorR.attach(PIN_MOTOR_RIGHT_1, PIN_MOTOR_RIGHT_2);
  motorMode(STOP, STOP, 0, 0);
  //センサー
  sensorInit();
  //エンコーダー
  pinMode(PIN_ROT, INPUT);
  //LED
  pinMode(PIN_LED, OUTPUT);
  digitalWrite(PIN_LED, HIGH);
  //モード
  mode = MODE_STOP;
  delay(1000);
  /* スタート */
  digitalWrite(PIN_LED, LOW);
  mode = MODE_TRACE;
  handle(0);
  lance(0);
  motorMode(GO, GO, 0xff, 0xff);
}

void loop(){
  //センサーを見る
  byte sensor = 0;
#ifdef LINE_BLACK //黒ライン
  sensor = PIN_SENSOR;
#else
  sensor = ~PIN_SENSOR;
#endif
  //ロータリーエンコーダーの変化を見る
  count();
  //ランスの動作を決定する
  switch(target){
  case TARGET_NONE:
    lanceAngle = LANCE_ANGLE0;
    if(counter - counterOld > TARGET_NONE_COUNT ) target = 0;
    break;
  default:
    switch( sensor & MASK_CHECK_MARKER ){
      case 0x81:
        target = TARGET_NONE;
        counterOld = counter;
        break;
      case 0x01:
        lanceAngle = LANCE_ANGLE1;
        target = TARGET_PARALLEL;
        break;
      case 0x80:
        lanceAngle = LANCE_ANGLE3;
        target = TARGET_VERTICAL;
        break;
    }
    break;
  }
  if( lanceAngle == LANCE_ANGLE1 && counter%840 > 420) lanceAngle = LANCE_ANGLE4;
  static unsigned long millis_lance = 0;
  if(millis() - millis_lance > LANCE_INTERVAL){
    millis_lance = millis();
    switch(lanceAngle){
      case LANCE_ANGLE1:
        lanceAngle = LANCE_ANGLE2;
        break;
      case LANCE_ANGLE2:
        lanceAngle = LANCE_ANGLE1;
        break;
    }
  }
  //全体の動作を決定する
  switch(mode){
  case MODE_TRACE:
    trace(sensor);
    if( counter > 854*6 - 60 ){     // 6周(1周854カウント)したら、円筒標的を狙いに行く
      mode = MODE_STOP;
    }
    break;
  case MODE_STOP:
  default:
    digitalWrite(PIN_LED, HIGH);
    handleAngle = 0;
    lanceAngle = -90;
    motorMode(GO, GO, 0xff, 0xff);
    if( counter - counterOld > 40 ){
      motorMode(STOP, STOP, 0xff, 0xff);
    }
  }
  
  handle(handleAngle);
  lance(lanceAngle);
}