変数名をキャメルケースに変更
This commit is contained in:
parent
6c6e9ac02b
commit
3c10dd5773
2 changed files with 245 additions and 210 deletions
377
hanzo4_1.ino
377
hanzo4_1.ino
|
|
@ -9,54 +9,88 @@
|
|||
#include <Servo.h>
|
||||
#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]);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -21,22 +21,80 @@ void lance(int a){
|
|||
|
||||
/* センサーのアレイの初期化 */
|
||||
void sensorInit(){
|
||||
for(int i=0; i<NUM_SENSORS; i++){
|
||||
pinMode(PIN_SENSOR[i], INPUT);
|
||||
for(int i=0; i<numSensors; i++){
|
||||
pinMode(sensorPin[i], INPUT);
|
||||
}
|
||||
}
|
||||
|
||||
/* センサーアレイの状態を見る*/
|
||||
/* センサーアレイの状態を見る */
|
||||
unsigned char sensorRead(){
|
||||
unsigned char b;
|
||||
//黒が1、白が0
|
||||
for(int i=0; i<NUM_SENSORS; i++){
|
||||
if(digitalRead(PIN_SENSOR[i])) b += 1<<i; //右端-左端
|
||||
for(int i=0; i<numSensors; i++){
|
||||
if(digitalRead(sensorPin[i])) b += 1<<i; //右端から順番に左端へ
|
||||
}
|
||||
b ^= 0xff; //今回ラインが白なので、反転して、ラインを1とする
|
||||
return sensor = b;
|
||||
}
|
||||
|
||||
/* センサーを読んでラインの位置を返す */
|
||||
int scanSensor(){
|
||||
int pos = 0;
|
||||
switch(sensor&lineSensorMask){
|
||||
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;
|
||||
lineDifferential = pos - linePosition;
|
||||
linePosition = pos;
|
||||
lineIntegral += pos;
|
||||
return pos;
|
||||
}
|
||||
|
||||
// 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
|
||||
int speedPIDControl(){
|
||||
return speedKP*linePosition + speedKI*lineIntegral + speedKD*lineDifferential;
|
||||
}
|
||||
|
||||
// ステアリングの角度(度)
|
||||
int steeringPIDControl(){
|
||||
return steeringKP*linePosition + steeringKI*lineIntegral + steeringKD*lineDifferential;
|
||||
}
|
||||
|
||||
/* エンコーダーの割り込み処理 */
|
||||
void rot(){
|
||||
distance++;
|
||||
|
|
@ -44,16 +102,16 @@ void rot(){
|
|||
|
||||
/* 起動音 */
|
||||
void startBeep(){
|
||||
tone(PIN_BUZZER,2000);
|
||||
tone(buzzerPin,2000);
|
||||
delay(100);
|
||||
tone(PIN_BUZZER,1000);
|
||||
tone(buzzerPin,1000);
|
||||
delay(100);
|
||||
noTone(PIN_BUZZER);
|
||||
noTone(buzzerPin);
|
||||
}
|
||||
|
||||
/* 終了音 */
|
||||
void stopBeep(){
|
||||
tone(PIN_BUZZER, 2000);
|
||||
tone(buzzerPin, 2000);
|
||||
delay(2000);
|
||||
noTone(PIN_BUZZER);
|
||||
noTone(buzzerPin);
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue