From 3c10dd5773605c9be750bb2e5c92740354a47dc2 Mon Sep 17 00:00:00 2001 From: kou029w Date: Wed, 19 Mar 2014 05:39:56 +0900 Subject: [PATCH] =?UTF-8?q?=E5=A4=89=E6=95=B0=E5=90=8D=E3=82=92=E3=82=AD?= =?UTF-8?q?=E3=83=A3=E3=83=A1=E3=83=AB=E3=82=B1=E3=83=BC=E3=82=B9=E3=81=AB?= =?UTF-8?q?=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- hanzo4_1.ino | 377 ++++++++++++++++++++++------------------------ hanzo4_1Utils.ino | 78 ++++++++-- 2 files changed, 245 insertions(+), 210 deletions(-) diff --git a/hanzo4_1.ino b/hanzo4_1.ino index 7159480..ecd8020 100644 --- a/hanzo4_1.ino +++ b/hanzo4_1.ino @@ -9,54 +9,88 @@ #include #include "Motor.h" -unsigned long LAP_DISTANCE = 28500; //ロータリーエンコーダーのカウント数/周 -unsigned long TURN_DISTANCE = 4650; //カーブ中でのカウント数 +/* 標的の直前にあるマーカーを見つけ出す範囲 : {最小, 最大} */ +struct range_t{ + unsigned long min; + unsigned long max; +}; -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; //度 +/* 標的のためのランスの角度 : {before[度], after[度]} */ +struct lanceAngle_t{ + int before; + int after; +}; -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; +/* 標的の情報 */ +struct target_t{ + struct range_t range; + unsigned long way; //マーカーから標的までの距離 + struct lanceAngle_t lanceAngle; +}; /**************************************/ +/* 動作パラメーター */ + +unsigned long lapDistance = 28500; //ロータリーエンコーダーのカウント数/周 +unsigned long turnDistance = 4650; //カーブ中でのカウント数 + +struct target_t targetA = {{lapDistance/2, lapDistance*0.6 }, 1500, { 29, 0}}; +struct target_t targetB = {{lapDistance/2, lapDistance*0.75}, 1500, {-32, 0}}; +struct target_t targetC = {{lapDistance*0.6, lapDistance*0.85}, 1500, { 34, 0}}; +struct target_t targetD = {{lapDistance*0.75, lapDistance*0.85}, 1500, {-42, 0}}; +struct target_t targetE = {{0, lapDistance/8 }, 800, { 40, 48}}; +struct target_t targetF = {{lapDistance/8, lapDistance/4 }, 1000, { 61, 69}}; + +int laps = 1; //最大周回数 +bool silent = true; //サイレントモード(true:ブザーを鳴らさない) + +int steeringAngleMin = -30; //度 +int steeringAngleMax = 30; //度 + +int speedMax = 100; //最大速度 + +float steeringKP = 3.8; +float steeringKI = 0.0; +float steeringKD = 0.0; + +float speedKP = 50.0; +float speedKI = 0.0; +float speedKD = 0.0; + +int errorCountThreshold = 5000; //最大読み取りエラー回数(これを超えると停止する) + +/**************************************/ +/* ピンの設定 */ + +// Servo : SC-1267SG +unsigned int steeringServoCenter = 1810; //us +unsigned int steeringServoMin = (steeringServoCenter-780); //us +unsigned int steeringServoMax = (steeringServoCenter+780); //us + +// Servo : SC-0352 +unsigned int lanceServoCenter = 1530; //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; @@ -64,23 +98,25 @@ Motor motorR; Motor motorL; //ラインが1、地面が0、LSBが左端、MSBが右端 -volatile unsigned char sensor = 0x00; +unsigned char sensor = 0x00; -volatile int linePosition[] = {0,0}; //{新,旧} -volatile long integral = 0; +int linePosition = 0; +int lineDifferential = 0; +long lineIntegral = 0; // 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター) -volatile int speed = 0; +int speed = 0; // ステアリングの角度 -volatile long steeringAngle = 0; // 度 +long angle = 0; // 度 -volatile int errorCount = 0; +int errorCount = 0; volatile unsigned long distance = 0; -volatile unsigned long modeChangedDistance = 0; +unsigned long modeChangedDistance = 0; /**************************************/ +/* 動作モード */ enum mode_t { MODE_STOP, //停止 @@ -97,131 +133,109 @@ enum mode_t { /**************************************/ -/* センサーを読んでラインの位置を返す */ -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; +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); + //ブザー + pinMode(buzzerPin, OUTPUT); + if(!silent) startBeep(); + /* 3秒停止 */ + motorL.mode(STOP); + motorR.mode(STOP); + delay(3000); + /* スタート */ + mode = MODE_STRAIGHT; + MsTimer2::set(1, run); + MsTimer2::start(); } -/* センサーを読んで、車体の動作を決定する(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); - +void loop(){ + sensorRead(); +} + +/* 車体の動作を決定する(1kHz) */ +void run(){ switch(mode){ default: case MODE_STOP: //停止 MsTimer2::stop(); - steering(-20); + steeringServo.detach(); motorL.mode(STOP); motorR.mode(STOP); - stopBeep(); + if(!silent) stopBeep(); while(1){ ; }; break; case MODE_STRAIGHT: //まっすぐ進む + trace(); modeSet(); break; case MODE_TURN: //左に曲がる + trace(); lance(0); - if(distance - modeChangedDistance > TURN_DISTANCE){ + if(distance - modeChangedDistance > turnDistance){ mode = MODE_STRAIGHT; } break; case MODE_ATTACK_E: //平行標的1 - lance(LANCE_ANGLE_E); - if(distance - modeChangedDistance > 800){ //マーカーから少し進んで、叩く - lance(LANCE_ANGLE_E + 8); + trace(); + lance(targetE.lanceAngle.before); + if(distance - modeChangedDistance > targetE.way){ //マーカーから少し進んで、叩く + lance(targetE.lanceAngle.after); mode = MODE_STRAIGHT; } break; case MODE_ATTACK_F: //平行標的2 - lance(LANCE_ANGLE_F); - if(distance - modeChangedDistance > 1000){ //マーカーから少し進んで、叩く - lance(LANCE_ANGLE_F + 8); + trace(); + lance(targetF.lanceAngle.before); + if(distance - modeChangedDistance > targetF.way){ //マーカーから少し進んで、叩く + lance(targetF.lanceAngle.after); mode = MODE_STRAIGHT; } break; case MODE_ATTACK_A: //右垂直標 - lance(LANCE_ANGLE_A); - if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す - lance(0); + trace(); + lance(targetA.lanceAngle.before); + if(distance - modeChangedDistance > targetA.way){ //マーカーから少し進んで、まっすぐに戻す + lance(targetA.lanceAngle.after); mode = MODE_STRAIGHT; } break; case MODE_ATTACK_C: //右垂直標 - lance(LANCE_ANGLE_C); - if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す - lance(0); + trace(); + lance(targetC.lanceAngle.before); + if(distance - modeChangedDistance > targetC.way){ //マーカーから少し進んで、まっすぐに戻す + lance(targetC.lanceAngle.after); mode = MODE_STRAIGHT; } break; case MODE_ATTACK_B: //左垂直標 - lance(LANCE_ANGLE_B); - if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す - lance(0); + trace(); + lance(targetB.lanceAngle.before); + if(distance - modeChangedDistance > targetB.way){ //マーカーから少し進んで、まっすぐに戻す + lance(targetB.lanceAngle.after); mode = MODE_STRAIGHT; } break; case MODE_ATTACK_D: //左垂直標 - lance(LANCE_ANGLE_D); - if(distance - modeChangedDistance > 1500){ //マーカーから少し進んで、まっすぐに戻す - lance(0); + trace(); + lance(targetD.lanceAngle.before); + if(distance - modeChangedDistance > targetD.way){ //マーカーから少し進んで、まっすぐに戻す + lance(targetD.lanceAngle.after); mode = MODE_STRAIGHT; } break; @@ -231,23 +245,33 @@ void trace(){ } } -// 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター) -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 trace(){ + scanSensor(); + + speed = speedPIDControl(); + speed = constrain(speed, -speedMax, speedMax); + if(speed<0){ + motorL.speed(speedMax + speed); + motorR.speed(speedMax); + }else{ + motorL.speed(speedMax); + motorR.speed(speedMax - speed); + } + + angle = steeringPIDControl(); + angle = constrain(angle, steeringAngleMin, steeringAngleMax); + steering(angle); } /* モードを決定する */ void modeSet(){ - if(distance > LAP_DISTANCE || errorCount > 5000){ + if(distance > lapDistance*laps || errorCount > errorCountThreshold){ mode = MODE_STOP; return; } - switch(sensor&MASK_MARKER){ + unsigned long way = distance%lapDistance; + switch(sensor&markerSensorMask){ default: case 0b00000000: mode = MODE_STRAIGHT; @@ -256,64 +280,17 @@ void modeSet(){ modeChangedDistance = distance; mode = MODE_TURN; break; - case 0b00000001: + case 0b00000001: //E,F,A,C 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; - } - } + 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: + case 0b10000000: //B,D 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; - } + 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; } } - -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]); -} diff --git a/hanzo4_1Utils.ino b/hanzo4_1Utils.ino index 2a8ea48..cbd7b4e 100644 --- a/hanzo4_1Utils.ino +++ b/hanzo4_1Utils.ino @@ -21,22 +21,80 @@ void lance(int a){ /* センサーのアレイの初期化 */ void sensorInit(){ - for(int i=0; i