69 lines
1.6 KiB
C++
69 lines
1.6 KiB
C++
/* ステアリングを中央からa[度]だけ動かす
|
|
<- - 0 + ->
|
|
||
|
|
||
|
|
[]---{}---[]
|
|
/ .\
|
|
*/
|
|
void steering(int a){
|
|
steeringServo.write(90 + a);
|
|
}
|
|
|
|
/* ランスを中央からa[度]だけ動かす
|
|
<- - 0 + ->
|
|
/ .\
|
|
/ | \
|
|
[]+--{}--+[]
|
|
*/
|
|
void lance(int a){
|
|
lanceServo.write(90 - a);
|
|
}
|
|
|
|
/* センサーのアレイの初期化 */
|
|
void sensorInit(){
|
|
pinMode(PIN_SENSOR_0, INPUT); //右端
|
|
pinMode(PIN_SENSOR_1, INPUT);
|
|
pinMode(PIN_SENSOR_2, INPUT);
|
|
pinMode(PIN_SENSOR_3, INPUT);
|
|
pinMode(PIN_SENSOR_4, INPUT);
|
|
pinMode(PIN_SENSOR_5, INPUT);
|
|
pinMode(PIN_SENSOR_6, INPUT);
|
|
pinMode(PIN_SENSOR_7, INPUT); //左端
|
|
}
|
|
|
|
/* センサーアレイの状態を見る*/
|
|
unsigned char sensorRead(){
|
|
unsigned char b;
|
|
//黒が1、白が0
|
|
if(digitalRead(PIN_SENSOR_0)) b += 0b00000001; //右端
|
|
if(digitalRead(PIN_SENSOR_1)) b += 0b00000010;
|
|
if(digitalRead(PIN_SENSOR_2)) b += 0b00000100;
|
|
if(digitalRead(PIN_SENSOR_3)) b += 0b00001000;
|
|
if(digitalRead(PIN_SENSOR_4)) b += 0b00010000;
|
|
if(digitalRead(PIN_SENSOR_5)) b += 0b00100000;
|
|
if(digitalRead(PIN_SENSOR_6)) b += 0b01000000;
|
|
if(digitalRead(PIN_SENSOR_7)) b += 0b10000000; //左端
|
|
b ^= 0xff; //今回ラインが白なので、反転して、ラインを1とする
|
|
return sensor = b;
|
|
}
|
|
|
|
/* エンコーダーの割り込み処理 */
|
|
void rot(){
|
|
distance++;
|
|
}
|
|
|
|
/* 起動音 */
|
|
void startBeep(){
|
|
tone(PIN_BUZZER,2000);
|
|
delay(100);
|
|
tone(PIN_BUZZER,1000);
|
|
delay(100);
|
|
noTone(PIN_BUZZER);
|
|
}
|
|
|
|
/* 終了音 */
|
|
void stopBeep(){
|
|
tone(PIN_BUZZER, 2000);
|
|
delay(2000);
|
|
noTone(PIN_BUZZER);
|
|
}
|