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

/*
     <前>
 
[0 1 2 3 4 5]
      |
      |
 []---*---[]
      | ,
      |/
 []---#---[]
 
*/

#define PIN_ANALOG_SENSOR_0 5
#define PIN_ANALOG_SENSOR_1 4
#define PIN_ANALOG_SENSOR_2 3
#define PIN_ANALOG_SENSOR_3 2
#define PIN_ANALOG_SENSOR_4 1
#define PIN_ANALOG_SENSOR_5 0

#define PIN_SERVO_HANDLE      10
#define PIN_SERVO_LANCE        9
#define PIN_MOTOR_RIGHT_SPEED  3
#define PIN_MOTOR_RIGHT_0      2
#define PIN_MOTOR_RIGHT_1      8
#define PIN_MOTOR_LEFT_SPEED   5
#define PIN_MOTOR_LEFT_0       7
#define PIN_MOTOR_LEFT_1       6 // 0:HIGH, 1:LOW => 正転 / 0:LOW, 1:HIGH => 逆転

#define STOP  0x00
#define GO    0b01
#define BACK  0b10
#define BRAKE 0b11

#define SPEED_MAX 255
#define SPEED_DEFAULT 255

#define HANDLE_DEFAULT 87 //度
#define HANDLE_MODE_LEFT  -25  //度
#define HANDLE_MODE_RIGHT  20 //度

#define LANCE_DEFAULT  95 //度

#define SENSOR_LIMIT 95 //センサーのしきい値
// センサー < SENSOR_LIMIT => 白

#define MODE_STOP    0x00
#define MODE_TRACE   0x10
#define MODE_RIGHT   0x21 // 右カーブ
#define MODE_LEFT    0x22 // 左カーブ

#define TARGET_NONE     0x40
#define TARGET_PARALLEL 0x41
#define TARGET_VERTICAL 0x42
#define TARGET_CYLINDER 0x43

#define MASK_MODE_TRACE          0b011110
#define MASK_MODE_RIGHT          0b001000
#define MASK_MODE_LEFT           0b000100
#define MASK_CHECK_MARKER        0b100001
#define MASK_CHECK_MARKER_RIGHT  0b000001
#define MASK_CHECK_MARKER_LEFT   0b100000

#define TIMER_INTERVAL 20 // timer()の実行間隔(ms)

//#define DEBUG //デバッグ有効(30秒停止機能)
//#define LINE_BLACK //白地・黒ライン有効

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

#include <Servo.h>
// #include <MsTimer2.h>

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

Servo servo_handle;
Servo servo_lance;

byte mode;
byte sensor_old;
//byte marker_old;
//byte target;
byte lance_old;
//byte marker;

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

byte sensor(){
  byte sensor_result = 0;

#ifdef LINE_BLACK
  if(analogRead(PIN_ANALOG_SENSOR_0) >= SENSOR_LIMIT){ //黒
    sensor_result += 0b100000 ;
  }
  if(analogRead(PIN_ANALOG_SENSOR_1) >= SENSOR_LIMIT){
    sensor_result += 0b010000 ;
  }
  if(analogRead(PIN_ANALOG_SENSOR_2) >= SENSOR_LIMIT){
    sensor_result += 0b001000 ;
  }
  if(analogRead(PIN_ANALOG_SENSOR_3) >= SENSOR_LIMIT){
    sensor_result += 0b000100 ;
  }
  if(analogRead(PIN_ANALOG_SENSOR_4) >= SENSOR_LIMIT){
    sensor_result += 0b000010 ;
  }
  if(analogRead(PIN_ANALOG_SENSOR_5) >= SENSOR_LIMIT){
    sensor_result += 0b000001 ;
  }
#else
  if(analogRead(PIN_ANALOG_SENSOR_0) < SENSOR_LIMIT){ //白
    sensor_result += 0b100000 ;
  }
  if(analogRead(PIN_ANALOG_SENSOR_1) < SENSOR_LIMIT){
    sensor_result += 0b010000 ;
  }
  if(analogRead(PIN_ANALOG_SENSOR_2) < SENSOR_LIMIT){
    sensor_result += 0b001000 ;
  }
  if(analogRead(PIN_ANALOG_SENSOR_3) < SENSOR_LIMIT){
    sensor_result += 0b000100 ;
  }
  if(analogRead(PIN_ANALOG_SENSOR_4) < SENSOR_LIMIT){
    sensor_result += 0b000010 ;
  }
  if(analogRead(PIN_ANALOG_SENSOR_5) < SENSOR_LIMIT){
    sensor_result += 0b000001 ;
  }
#endif

  return sensor_result;
}

/*
bool check_marker(){
  bool check_marker_result = false;
  if(sensor(MASK_CHECK_MARKER)) check_marker_result = true;
  return check_marker_result;
}

bool check_marker_right(){
  bool check_marker_result = false;
  if(sensor(MASK_CHECK_MARKER_RIGHT)) check_marker_result = true;
  return check_marker_result;
}

bool check_marker_left(){
  bool check_marker_result = false;
  if(sensor(MASK_CHECK_MARKER_LEFT)) check_marker_result = true;
  return check_marker_result;
}
*/

void motorSpeed(byte motor_left_speed, byte motor_right_speed){
  analogWrite(PIN_MOTOR_LEFT_SPEED, motor_left_speed);
  analogWrite(PIN_MOTOR_RIGHT_SPEED, motor_right_speed);
}

void motorMode(byte motor_left_mode, byte motor_right_mode){
  digitalWrite(PIN_MOTOR_LEFT_0,  (  motor_left_mode >> 0 ) & 0x01 );
  digitalWrite(PIN_MOTOR_LEFT_1,  (  motor_left_mode >> 1 ) & 0x01 );
  digitalWrite(PIN_MOTOR_RIGHT_0, ( motor_right_mode >> 0 ) & 0x01 );
  digitalWrite(PIN_MOTOR_RIGHT_1, ( motor_right_mode >> 1 ) & 0x01 );
}

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

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

#ifdef DEBUG
void timer(unsigned long millis_now){
  if(millis_now > 30 * 1000){
    mode = MODE_STOP;
  }
}
#endif

void setup() {

  pinMode(PIN_ANALOG_SENSOR_0, INPUT);
  pinMode(PIN_ANALOG_SENSOR_1, INPUT);
  pinMode(PIN_ANALOG_SENSOR_2, INPUT);
  pinMode(PIN_ANALOG_SENSOR_3, INPUT);
  pinMode(PIN_ANALOG_SENSOR_4, INPUT);
  pinMode(PIN_ANALOG_SENSOR_5, INPUT);

  pinMode(PIN_MOTOR_LEFT_SPEED, OUTPUT);
  pinMode(PIN_MOTOR_LEFT_0, OUTPUT);
  pinMode(PIN_MOTOR_LEFT_1, OUTPUT);

  pinMode(PIN_MOTOR_RIGHT_SPEED, OUTPUT);
  pinMode(PIN_MOTOR_RIGHT_0, OUTPUT);
  pinMode(PIN_MOTOR_RIGHT_1, OUTPUT);

  motorSpeed(0, 0);
  motorMode(STOP, STOP);

  servo_handle.attach(PIN_SERVO_HANDLE);
  handle(0);

  servo_lance.attach(PIN_SERVO_LANCE);
  lance(0);

  mode = MODE_STOP;

  //  target = TARGET_NONE;
  //  marker =0;

  /*
  MsTimer2::set(TIMER_INTERVAL, timer);
   MsTimer2::start();
   */

  //--

  delay(1000);
  //  Serial.begin(9600);
  mode = MODE_TRACE;
  handle(0);
  motorMode(GO, GO);
  motorSpeed(SPEED_DEFAULT, SPEED_DEFAULT);

}

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

void loop() {
  sensor_old = sensor();
  
  switch(mode){
    case MODE_STOP:
      lance(0);
      handle(0);
      motorMode(BRAKE, BRAKE);
      motorSpeed(0, 0);
    break;

    case MODE_TRACE:
    
      static unsigned long millis_lance = 0;
      if(millis() - millis_lance > 70){
        millis_lance = millis();
        
        switch(lance_old){
          case 39:
            lance_old = 29;
            break;
          case 29:
            lance_old = 39;
            break;
        }
      }
      
      if( (sensor_old >> 5) & 0x01){
        lance_old = -33;
      }
      
      if( (sensor_old >> 0) & 0x01){
        lance_old = 39;
      }
      
      lance(lance_old);
   // handle(0);
  //  motorMode(GO, GO);
  //  motorSpeed(SPEED_DEFAULT, SPEED_DEFAULT);
    /*
      switch(sensor(MASK_CHECK_MARKER)){
     case 0b000001:
     marker = 0x01;
     break;
     case 0b100000:
     marker = 0x02;
     break;
     }
     */
      switch(sensor_old){
   // case 0b000000:
   //   handle(0);
   //   motorMode(GO, GO);
   //   motorSpeed(SPEED_DEFAULT, SPEED_DEFAULT);
   //   break;

        case 0b001000:
          handle(-3);
          motorMode(GO, GO);
          motorSpeed(SPEED_DEFAULT, SPEED_DEFAULT);
        break;

        case 0b000100:
          handle(3);
          motorMode(GO, GO);
          motorSpeed(SPEED_DEFAULT,SPEED_DEFAULT);
        break;

        case 0b010000:
  //      delay(0);
          mode = MODE_LEFT;
    //  handle(HANDLE_MODE_LEFT);
    //  motorMode(GO, GO);
    //  motorSpeed(SPEED_DEFAULT*0.7, SPEED_DEFAULT);
        break;

        case 0b000010:
          handle(12);
          motorMode(GO, GO);
          motorSpeed(SPEED_DEFAULT,SPEED_DEFAULT);
          //mode = MODE_RIGHT;
        break;

      }
    
    break;

  case MODE_LEFT:
    
    switch(sensor_old){
      case 0b010000:
        handle(HANDLE_MODE_LEFT);
        motorMode(BRAKE, GO);
        motorSpeed(0, SPEED_DEFAULT);
      break;
/*    case 0b001000:
      handle(HANDLE_MODE_LEFT);
      motorMode(GO, GO);
      motorSpeed(SPEED_DEFAULT*0.7, SPEED_DEFAULT);
      break;
      */
      case 0b000100:
        mode = MODE_TRACE;
      break;
    }
    break;
    /*
    case MODE_RIGHT:
     handle(HANDLE_MODE_RIGHT);
     motorMode(GO, GO);
     motorSpeed(SPEED_DEFAULT, SPEED_DEFAULT*0.8);
     switch(sensor(MASK_MODE_RIGHT)){
     case 0b001000:
     mode = MODE_TRACE;
     break;
     }
     break;
     */
  }
#ifdef DEBUG
  static unsigned long millis_old = 0;
  if(millis() - millis_old > TIMER_INTERVAL){
    millis_old = millis();
    timer(millis_old);
  }
#endif
}