/* 競技用ランサーロボット 半蔵 4.1 (C)2014 kou029w - MIT License */ #include #include #include #include "Motor.h" /* 標的の直前にあるマーカーを見つけ出す範囲 : {最小, 最大} */ struct range_t{ unsigned long min; unsigned long max; }; /* 標的のためのランスの角度 : {before[度], after[度]} */ struct lanceAngle_t{ int before; int after; }; /* 標的の情報 */ struct target_t{ struct range_t range; unsigned long way; //マーカーから標的までの距離 struct lanceAngle_t lanceAngle; }; /**************************************/ /* 動作パラメーター */ unsigned long lapDistance = 28550; //ロータリーエンコーダーのカウント数/周 unsigned long turnDistance = 4650; //カーブ中でのカウント数 struct target_t targetA = {{lapDistance/2, lapDistance*0.6 }, 900, { 29, 0}}; struct target_t targetB = {{lapDistance/2, lapDistance*0.75}, 900, {-32, 0}}; struct target_t targetC = {{lapDistance*0.6, lapDistance*0.85}, 900, { 34, 0}}; struct target_t targetD = {{lapDistance*0.75, lapDistance*0.85}, 900, {-42, 0}}; struct target_t targetE = {{ 500,4000}, 750, { 48, 53}}; struct target_t targetF = {{4000,7000}, 750, { 60, 67}}; int laps = 3; //最大周回数 bool silent = false; //サイレントモード(true:ブザーを鳴らさない) int steeringAngleMin = -30; //度 int steeringAngleMax = 10; //度 int deltaSpeedMax = 500; //最大速度差 int speedMax = 255; //最大速度 int cycle = 1; //動作周波数(ms) int errorCountThreshold = 500; //最大読み取りエラー回数(これを超えると停止する) /**************************************/ /* ピンの設定 */ // Servo : SC-1267SG unsigned int steeringServoCenter = 1808; //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 buzzerPin = 3; unsigned char rotPin = 2; unsigned char numSensors = 8; unsigned char sensorPin[] = {19,18,17,16,15,14,13,12}; //右端から順番に左端へ unsigned char lineSensorMask = 0b01111110; unsigned char markerSensorMask = 0b10000001; unsigned char steeringServoPin = 9; unsigned char lanceServoPin = 10; unsigned char motorLeftEnablePin = 5; unsigned char motorLeftDirectionPin = 4; unsigned char motorRightEnablePin = 6; unsigned char motorRightDirectionPin = 7; /**************************************/ /* グローバル変数 */ Servo steeringServo; Servo lanceServo; Motor motorR; Motor motorL; //ラインが1、地面が0、MSBが左端、LSBが右端 unsigned char sensor = 0x00; // 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター) int deltaSpeed = 0; // 駆動用モーターの速度 int speed = 0; // ステアリングの角度 long angle = 0; // 度 int errorCount = 0; volatile unsigned long distance = 0; 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; /**************************************/ /* センサーを読んで、車体の動作を決定する */ void trace(){ switch(sensor&lineSensorMask){ default: errorCount++; return; case 0b01000000: speed = speedMax*0.3; deltaSpeed = -145; angle = -15; break; case 0b01100000: speed = speedMax*0.5; deltaSpeed = -115; angle = -13; break; case 0b00100000: speed = speedMax*0.7; deltaSpeed = -95; angle = -11; break; case 0b00110000: speed = speedMax*0.9; deltaSpeed = -75; angle = -9; break; case 0b00010000: speed = speedMax; deltaSpeed = 0; angle = -1; break; case 0b00011000: speed = speedMax; deltaSpeed = 0; angle = 0; break; case 0b00001000: speed = speedMax; deltaSpeed = 0; angle = 1; break; case 0b00001100: speed = speedMax; deltaSpeed = 0; angle = 2; break; case 0b00000100: speed = speedMax; deltaSpeed = 3; angle = 3; break; case 0b00000110: speed = speedMax; deltaSpeed = 4; angle = 4; break; case 0b00000010: speed = 0; deltaSpeed = 5; angle = 5; break; } errorCount = 0; } /**************************************/ void setup(){ //サーボ steeringServo.attach(steeringServoPin, steeringServoMin, steeringServoMax); lanceServo.attach(lanceServoPin, lanceServoMin, lanceServoMax); steering(0); lance(0); //駆動用モーター motorL.attach(motorLeftEnablePin, motorLeftDirectionPin); motorR.attach(motorRightEnablePin, motorRightDirectionPin); motorL.mode(STOP); motorR.mode(STOP); //センサー sensorInit(); //エンコーダー pinMode(rotPin, INPUT); attachInterrupt(0, rot, RISING); //ブザー startBeep(); /* 3秒停止 */ delay(3000); /* スタート */ mode = MODE_STRAIGHT; MsTimer2::set(cycle, run); MsTimer2::start(); } void loop(){ scanSensor(); } /* 車体の動作を決定する */ void run(){ if(distance > lapDistance*laps || errorCount > errorCountThreshold){ mode = MODE_STOP; } switch(mode){ default: case MODE_STOP: //停止 MsTimer2::stop(); lanceServo.detach(); steeringServo.detach(); motorL.mode(STOP); motorR.mode(STOP); while(1){ ; } break; case MODE_STRAIGHT: //まっすぐ進む trace(); modeSet(); break; case MODE_TURN: //左に曲がる trace(); lance(0); if(distance - modeChangedDistance > turnDistance){ mode = MODE_STRAIGHT; } break; case MODE_ATTACK_E: //平行標的1 trace(); lance(targetE.lanceAngle.before); if(distance - modeChangedDistance > targetE.way){ //マーカーから少し進んで、叩く lance(targetE.lanceAngle.after); mode = MODE_STRAIGHT; } break; case MODE_ATTACK_F: //平行標的2 trace(); lance(targetF.lanceAngle.before); if(distance - modeChangedDistance > targetF.way){ //マーカーから少し進んで、叩く lance(targetF.lanceAngle.after); mode = MODE_STRAIGHT; } break; case MODE_ATTACK_A: //右垂直標 trace(); lance(targetA.lanceAngle.before); if(distance - modeChangedDistance > targetA.way){ //マーカーから少し進んで、まっすぐに戻す lance(targetA.lanceAngle.after); mode = MODE_STRAIGHT; } break; case MODE_ATTACK_C: //右垂直標 trace(); lance(targetC.lanceAngle.before); if(distance - modeChangedDistance > targetC.way){ //マーカーから少し進んで、まっすぐに戻す lance(targetC.lanceAngle.after); mode = MODE_STRAIGHT; } break; case MODE_ATTACK_B: //左垂直標 trace(); lance(targetB.lanceAngle.before); if(distance - modeChangedDistance > targetB.way){ //マーカーから少し進んで、まっすぐに戻す lance(targetB.lanceAngle.after); mode = MODE_STRAIGHT; } break; case MODE_ATTACK_D: //左垂直標 trace(); lance(targetD.lanceAngle.before); if(distance - modeChangedDistance > targetD.way){ //マーカーから少し進んで、まっすぐに戻す lance(targetD.lanceAngle.after); mode = MODE_STRAIGHT; } break; case MODE_ATTACK_CYLINDER: //円筒標的 mode = MODE_STRAIGHT; break; } /* 駆動モーター用 */ deltaSpeed = constrain(deltaSpeed, -deltaSpeedMax, deltaSpeedMax); if(deltaSpeed<0){ motorL.speed(speed + deltaSpeed); motorR.speed(speed); }else{ motorL.speed(speed); motorR.speed(speed - deltaSpeed); } /* ステアリングサーボ用 */ angle = constrain(angle, steeringAngleMin, steeringAngleMax); steering(angle); } /* モードを決定する */ void modeSet(){ unsigned long way = distance%lapDistance; switch(sensor&markerSensorMask){ default: case 0b00000000: mode = MODE_STRAIGHT; break; case 0b10000001: modeChangedDistance = distance; mode = MODE_TURN; break; case 0b00000001: //E,F,A,C modeChangedDistance = distance; if(targetE.range.min <= way && way < targetE.range.max) mode = MODE_ATTACK_E; if(targetF.range.min <= way && way < targetF.range.max) mode = MODE_ATTACK_F; if(targetA.range.min <= way && way < targetA.range.max) mode = MODE_ATTACK_A; if(targetC.range.min <= way && way < targetC.range.max) mode = MODE_ATTACK_C; break; case 0b10000000: //B,D modeChangedDistance = distance; if(targetB.range.min <= way && way < targetB.range.max) mode = MODE_ATTACK_B; if(targetD.range.min <= way && way < targetD.range.max) mode = MODE_ATTACK_D; break; } }