#include <Servo.h>

// Servo : SC-1267SG
unsigned int steeringServoCenter = 1820;  //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 rotPin     = 2;

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

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

volatile unsigned long distance = 0;

Servo lanceServo;
Servo steeringServo;

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

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

/* センサーのアレイの初期化 */
void sensorInit(){
  for(int i=0; i<numSensors; i++){
    pinMode(sensorPin[i], INPUT);
  }
}

/* エンコーダーの割り込み処理 */
void rot(){
  distance++;
}

void setup() {
  steeringServo.attach(steeringServoPin, steeringServoMin, steeringServoMax);
  lanceServo.attach(lanceServoPin, lanceServoMin, lanceServoMax);
  steering(0);
  lance(0);
  pinMode(rotPin, INPUT);
  attachInterrupt(0, rot, RISING);
  Serial.begin(9600);
  Serial.println("format : [+-][0-180][hl]");
}

void loop() {
  static int i = 0;
  i++;
  if(i>100){
    i = 0;
    Serial.print(distance);
    Serial.print('\t');
    for(int i=0; i<numSensors; i++){
      Serial.print(digitalRead(sensorPin[i]));
    }
    Serial.println();
  }
  
  static int v = 0;
  static int s = 1;
  
  if ( Serial.available() ) {
    char ch = Serial.read();
    switch(ch) {
      case '-':
        s = -1;
        break;
      case '0'...'9':
        v = v * 10 + ch - '0';
        break;
      case 'l':
        v = s*v;
        Serial.print("lance :");
        Serial.println(v);
        lance(v);
        v = 0;
        s = 1;
        break;
      case 'h':
        v = s*v;
        Serial.print("steering :");
        Serial.println(v);
        steering(v);
        v = 0;
        s = 1;
        break;
      case 'c':
        Serial.println("clear");
        distance = 0;
        v = 0;
        s = 1;
        break;
    }
  }
  
  delay(1);
}