127 lines
2.4 KiB
C++
127 lines
2.4 KiB
C++
/* ステアリングを中央からa[度]だけ動かす
|
|
<- - 0 + ->
|
|
||
|
|
||
|
|
[]---{}---[]
|
|
/ .\
|
|
*/
|
|
void steering(int a){
|
|
steeringServo.write(90 + a);
|
|
}
|
|
|
|
/* ランスを中央からa[度]だけ動かす
|
|
<- - 0 + ->
|
|
/ .\
|
|
/ | \
|
|
[]+--{}--+[]
|
|
*/
|
|
void lance(int a){
|
|
lanceServo.write(90 - a);
|
|
}
|
|
|
|
/* センサーのアレイの初期化 */
|
|
void sensorInit(){
|
|
for(int i=0; i<numSensors; i++){
|
|
pinMode(sensorPin[i], INPUT);
|
|
}
|
|
}
|
|
|
|
/* センサーアレイの状態を見る */
|
|
unsigned char sensorRead(){
|
|
unsigned char b;
|
|
//黒が1、白が0
|
|
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;
|
|
|
|
static int forDiv16;
|
|
if(forDiv16<16){
|
|
lineDifferential += pos - linePosition;
|
|
forDiv16++;
|
|
}else{
|
|
lineDifferential16 = lineDifferential;
|
|
lineDifferential = 0;
|
|
forDiv16 = 0;
|
|
}
|
|
linePosition = pos;
|
|
lineIntegral += pos;
|
|
return pos;
|
|
}
|
|
|
|
// 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
|
|
int speedPIDControl(){
|
|
return speedKP*linePosition + speedKI*lineIntegral + speedKD*lineDifferential16/16;
|
|
}
|
|
|
|
// ステアリングの角度(度)
|
|
int steeringPIDControl(){
|
|
return steeringKP*linePosition + steeringKI*lineIntegral + steeringKD*lineDifferential16/16;
|
|
}
|
|
|
|
/* エンコーダーの割り込み処理 */
|
|
void rot(){
|
|
distance++;
|
|
}
|
|
|
|
/* 起動音 */
|
|
void startBeep(){
|
|
tone(buzzerPin,2000);
|
|
delay(100);
|
|
tone(buzzerPin,1000);
|
|
delay(100);
|
|
noTone(buzzerPin);
|
|
}
|
|
|
|
/* 終了音 */
|
|
void stopBeep(){
|
|
tone(buzzerPin, 2000);
|
|
delay(2000);
|
|
noTone(buzzerPin);
|
|
}
|