2014-git-work/arduino/hanzo2/hanzo2.ino

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

/*
    <前>

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

//#define LINE_BLACK //白地・黒ライン有効

#define TARGET_NONE_COUNT 85

#define LANCE_ANGLE0     0 //度
#define LANCE_ANGLE1    43
#define LANCE_ANGLE2    54
#define LANCE_ANGLE3   -33
#define LANCE_ANGLE4    34
#define LANCE_INTERVAL  70 //ms

#define PIN_LED 4
#define PIN_ROT 2

#define PIN_SENSOR_0 12
#define PIN_SENSOR_1 13
#define PIN_SENSOR_2 14
#define PIN_SENSOR_3 15
#define PIN_SENSOR_4 16
#define PIN_SENSOR_5 17
#define PIN_SENSOR_6 18
#define PIN_SENSOR_7 19

#define PIN_SENSOR ( ((PINC<<2) & ~0x03) | ((PINB>>4) & 0x03) )

#define PIN_SERVO_HANDLE      10
#define PIN_SERVO_LANCE        9

#define PIN_MOTOR_RIGHT_1      6
#define PIN_MOTOR_RIGHT_2      8
#define PIN_MOTOR_LEFT_1       5
#define PIN_MOTOR_LEFT_2       7 // ex) RIGHT_1:HIGH, RIGHT_2:LOW => 正転 / RIGHT_1:LOW, RIGHT_2:HIGH => 逆転

#define STOP  0
#define GO    1
#define BACK  2
#define BRAKE 3

#define SPEED_DEFAULT 0xff

#define HANDLE_DEFAULT 90 //度
#define HANDLE_MIN ( 800+30) //ms
#define HANDLE_MAX (2300+30) //ms

#define LANCE_DEFAULT  90 //度
#define LANCE_MIN  544 //ms
#define LANCE_MAX 2400 //ms

#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          0b01111110
#define MASK_CHECK_MARKER        0b10000001
#define MASK_CHECK_MARKER_RIGHT  0b00000001
#define MASK_CHECK_MARKER_LEFT   0b10000000

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

#include <Servo.h>

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

Servo servo_handle;
Servo servo_lance;

byte mode;
byte target;
unsigned int counter_old;
char handle_angle;
char lance_angle;

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

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

void motorMode(byte left_mode, byte right_mode, byte left_speed, byte right_speed){
  analogWrite(PIN_MOTOR_LEFT_1 , left_speed  * (left_mode  & 0x01) );
  analogWrite(PIN_MOTOR_RIGHT_1, right_speed * (right_mode & 0x01) );
  digitalWrite(PIN_MOTOR_LEFT_2 , left_mode  >> 1 );
  digitalWrite(PIN_MOTOR_RIGHT_2, right_mode >> 1 );
}

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

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

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

void setup() {
  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);
  
  pinMode(PIN_MOTOR_RIGHT_1, OUTPUT);
  pinMode(PIN_MOTOR_RIGHT_2, OUTPUT);
  pinMode(PIN_MOTOR_LEFT_1 , OUTPUT);
  pinMode(PIN_MOTOR_LEFT_2 , OUTPUT);
  
  pinMode(PIN_ROT, INPUT);
  
  motorMode(STOP, STOP, 0, 0);

  servo_handle.attach(PIN_SERVO_HANDLE, HANDLE_MIN, HANDLE_MAX);
  handle(0);
  
  servo_lance.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX);
  lance(0);

  mode = MODE_STOP;
  
  digitalWrite(PIN_LED, HIGH);

  delay(1000);

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

  digitalWrite(PIN_LED, LOW);
  mode = MODE_TRACE;
  handle(0);
  motorMode(GO, GO, 0xff, 0xff);

}

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

void loop(){

  count(); //ロータリーエンコーダーの変化を見る

  static byte sensor = 0;
#ifdef LINE_BLACK //黒ライン
  sensor = PIN_SENSOR;
#else
  sensor = ~PIN_SENSOR;
#endif

  switch(target){
    case TARGET_NONE:
      lance_angle = LANCE_ANGLE0;
      if(counter - counter_old > TARGET_NONE_COUNT ) target = 0;
      break;
    default:
      switch( sensor & MASK_CHECK_MARKER ){
        case 0x81:
          target = TARGET_NONE;
          counter_old = counter;
          break;
        case 0x01:
          lance_angle = LANCE_ANGLE1;
          target = TARGET_PARALLEL;
          break;
        case 0x80:
          lance_angle = LANCE_ANGLE3;
          target = TARGET_VERTICAL;
          break;
      }
      break;
  }
  if( lance_angle == LANCE_ANGLE1 && counter%840 > 420) lance_angle = LANCE_ANGLE4;
  static unsigned long millis_lance = 0;
  if(millis() - millis_lance > LANCE_INTERVAL){
    millis_lance = millis();
    switch(lance_angle){
      case LANCE_ANGLE1:
        lance_angle = LANCE_ANGLE2;
        break;
      case LANCE_ANGLE2:
        lance_angle = 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);
      handle_angle = 0;
      lance_angle = -90;
      motorMode(GO, GO, 0xff, 0xff);
      if( counter - counter_old > 40 ){
        motorMode(STOP, STOP, 0xff, 0xff);
      }
  }
  
  handle(handle_angle);
  lance(lance_angle);

}