hanzo/hanzo4_1.ino

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

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

unsigned long LAP_DISTANCE  = 28500; //ロータリーエンコーダーのカウント数/周
unsigned long TURN_DISTANCE = 4650;  //カーブ中でのカウント数

int LANCE_ANGLE_E =  40;  //度
int LANCE_ANGLE_F =  61;  //度
int LANCE_ANGLE_A =  29;  //度
int LANCE_ANGLE_B = -32;  //度
int LANCE_ANGLE_C =  34;  //度
int LANCE_ANGLE_D = -42;  //度

int SPEED_DEFAULT = 100;

float STEERING_KP = 3.8;
float STEERING_KI = 0.0;
float STEERING_KD = 0.0;

float SPEED_KP = 50.0;
float SPEED_KI = 0.0;
float SPEED_KD = 0.0;

// Servo : SC-1267SG
unsigned int STEERING_CENTER = 1810;  //us
unsigned int STEERING_MIN = (STEERING_CENTER-780); //us
unsigned int STEERING_MAX = (STEERING_CENTER+780); //us

// Servo : SC-0352
unsigned int LANCE_CENTER = 1530;  //us
unsigned int LANCE_MIN = (LANCE_CENTER-780); //us
unsigned int LANCE_MAX = (LANCE_CENTER+780); //us

unsigned char PIN_BUZZER =  3;
unsigned char PIN_ROT    =  2;

unsigned char NUM_SENSORS = 8;
unsigned char PIN_SENSOR[] = {19,18,17,16,15,14,13,12};  //右端-左端

unsigned char PIN_SERVO_STEERING  =  9;
unsigned char PIN_SERVO_LANCE     = 10;

unsigned char PIN_MOTOR_LEFT_E  =  5;
unsigned char PIN_MOTOR_LEFT_M  =  4;
unsigned char PIN_MOTOR_RIGHT_E =  6;
unsigned char PIN_MOTOR_RIGHT_M =  7;

unsigned char MASK_LINE    = 0b01111110;
unsigned char MASK_MARKER  = 0b10000001;

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

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

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

volatile int linePosition[] = {0,0}; //{新,旧}
volatile long integral = 0;

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

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

volatile int errorCount = 0;

volatile unsigned long distance = 0;
volatile 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;

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

/* センサーを読んでラインの位置を返す */
int scanSensor(){
  sensorRead();
  int pos = 0;
  switch(sensor&MASK_LINE){
  case 0b01000000:
    pos = -5;
    break;
  case 0b01100000:
    pos = -4;
    break;
  case 0b00100000:
    pos = -3;
    break;
  case 0b00110000:
    pos = -2;
    break;
  case 0b00010000:
    pos = -1;
    break;
  case 0b00011000:
    pos = 0;
    break;
  case 0b00001000:
    pos = 1;
    break;
  case 0b00001100:
    pos = 2;
    break;
  case 0b00000100:
    pos = 3;
    break;
  case 0b00000110:
    pos = 4;
    break;
  case 0b00000010:
    pos = 5;
    break;
  default:
    errorCount++;
    return 127;
  }
  errorCount = 0;
  linePosition[1] = linePosition[0];
  linePosition[0] = pos;
  integral += (linePosition[0] + linePosition[1])>>1;
  return pos;
}

/* センサーを読んで、車体の動作を決定する(1kHz) */
void trace(){
  speed = speedPDControl();
  speed = constrain(speed, -SPEED_DEFAULT, SPEED_DEFAULT);
  if(speed<0){
    motorL.speed(SPEED_DEFAULT + speed);
    motorR.speed(SPEED_DEFAULT);
  }else{
    motorL.speed(SPEED_DEFAULT);
    motorR.speed(SPEED_DEFAULT - speed);
  }
  
  steeringAngle = steeringPDControl();
  steeringAngle = constrain(steeringAngle, -25, 25);
  steering(steeringAngle);
  
  switch(mode){
  default:
  case MODE_STOP:      //停止
    MsTimer2::stop();
    steering(-20);
    motorL.mode(STOP);
    motorR.mode(STOP);
    stopBeep();
    while(1){
      ;
    };
    break;
  case MODE_STRAIGHT:  //まっすぐ進む
    modeSet();
    break;
  case MODE_TURN:      //左に曲がる
    lance(0);
    if(distance - modeChangedDistance > TURN_DISTANCE){
      mode = MODE_STRAIGHT;
    }
    break;
  case MODE_ATTACK_E:  //平行標的1
    lance(LANCE_ANGLE_E);
    if(distance - modeChangedDistance > 800){ //マーカーから少し進んで、叩く
      lance(LANCE_ANGLE_E + 8);
      mode = MODE_STRAIGHT;
    }
    break;
  case MODE_ATTACK_F:  //平行標的2
    lance(LANCE_ANGLE_F);
    if(distance - modeChangedDistance > 1000){ //マーカーから少し進んで、叩く
      lance(LANCE_ANGLE_F + 8);
      mode = MODE_STRAIGHT;
    }
    break;
  case MODE_ATTACK_A:  //右垂直標
    lance(LANCE_ANGLE_A);
    if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す
      lance(0);
      mode = MODE_STRAIGHT;
    }
    break;
  case MODE_ATTACK_C:  //右垂直標
    lance(LANCE_ANGLE_C);
    if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す
      lance(0);
      mode = MODE_STRAIGHT;
    }
    break;
  case MODE_ATTACK_B:  //左垂直標
    lance(LANCE_ANGLE_B);
    if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す
      lance(0);
      mode = MODE_STRAIGHT;
    }
    break;
  case MODE_ATTACK_D:  //左垂直標
    lance(LANCE_ANGLE_D);
    if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す
      lance(0);
      mode = MODE_STRAIGHT;
    }
    break;
  case MODE_ATTACK_CYLINDER:    //円筒標的
    mode = MODE_STRAIGHT;
    break;
  }
}

// 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
int speedPDControl(){
  return SPEED_KP*linePosition[0] + SPEED_KI*integral + SPEED_KD*(linePosition[0] - linePosition[1]);
}

// ステアリングの角度(度)
int steeringPDControl(){
  return STEERING_KP*linePosition[0] + STEERING_KI*integral + STEERING_KD*(linePosition[0] - linePosition[1]);
}

/* モードを決定する */
void modeSet(){
  if(distance > LAP_DISTANCE || errorCount > 5000){
    mode = MODE_STOP;
    return;
  }
  switch(sensor&MASK_MARKER){
  default:
  case 0b00000000:
    mode = MODE_STRAIGHT;
    break;
  case 0b10000001:
    modeChangedDistance = distance;
    mode = MODE_TURN;
    break;
  case 0b00000001:
    modeChangedDistance = distance;
    if(distance%LAP_DISTANCE < LAP_DISTANCE/8){  //まずは、E的をねらいたい
      mode = MODE_ATTACK_E;
    }else if(distance%LAP_DISTANCE < LAP_DISTANCE/4){  //次に、F的をねらいたい
      mode = MODE_ATTACK_F;
    }else if(distance%LAP_DISTANCE > LAP_DISTANCE/2){  //後半は垂直標的をねらいたい
      if(distance%LAP_DISTANCE < LAP_DISTANCE*0.6){
        mode = MODE_ATTACK_A;
      }else if(distance%LAP_DISTANCE < LAP_DISTANCE*0.85){
        mode = MODE_ATTACK_C;
      }
    }
    break;
  case 0b10000000:
    modeChangedDistance = distance;
    if(distance%LAP_DISTANCE < LAP_DISTANCE/2){
      ;
    }else if(distance%LAP_DISTANCE < LAP_DISTANCE*0.75){
      mode = MODE_ATTACK_B;
    }else if(distance%LAP_DISTANCE < LAP_DISTANCE*0.85){
      mode = MODE_ATTACK_D;
    }
    break;
  }
}

void setup(){
  Serial.begin(9600);
  //サーボ
  steeringServo.attach(PIN_SERVO_STEERING, STEERING_MIN, STEERING_MAX);
  lanceServo.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX);
  steering(0);
  lance(0);
  //モーター
  motorL.attach(PIN_MOTOR_LEFT_E, PIN_MOTOR_LEFT_M);
  motorR.attach(PIN_MOTOR_RIGHT_E, PIN_MOTOR_RIGHT_M);
  motorL.mode(STOP);
  motorR.mode(STOP);
  //センサー
  sensorInit();
  //エンコーダー
  pinMode(PIN_ROT, INPUT);
  attachInterrupt(0, rot, RISING);
  //ブザー
  pinMode(PIN_BUZZER, OUTPUT);
//  startBeep();
  /* 3秒停止 */
  motorL.mode(STOP);
  motorR.mode(STOP);
  delay(3000);
  /* スタート */
  mode = MODE_STRAIGHT;
  MsTimer2::set(1, trace);
  MsTimer2::start();
}

void loop(){
  scanSensor();
  Serial.println(linePosition[0]);
}