/* 競技用ランサーロボット 半蔵 4.1 (C)2014 kou029w - MIT License */ #include #include #include #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]); }