うまく作動しているようです。この後は試験運転を続けて不具合を調整していくことになります。心配していた電池の寿命ですがフル充電した2300mAh のCR232電池二個で約8分間の連続走行は可能ですが、最終的には二個追加しました。
//Self Driving Vehicle ver.2 2017.12.30
//赤外線距離センサ(GP2YOA21)周囲の障害物検知
//赤外線近接センサ(LM393) 障害物検知の補助として二個、斜め下方と低位置前方を測距
#include <VarSpeedServo.h>
// サーボを定義
VarSpeedServo servo1,servo2,servo3;
int servo1_min,servo1_max,servo1_mean; //首振り
int servo2_min,servo2_max,servo2_mean; //首傾げ
int servo3_min,servo3_max,servo3_mean; //首上下
int position_of_servo1[] ={85,30,85,140};
int len_1 = 4;
int position_of_servo2[] ={60,80};
int len_2 = 2;
int position_of_servo3[] ={60,100};
int len_3 = 2;
// モーターを定義
const int motorA_1 = 7;
const int motorA_2 = 8;
const int PWM_motorA = 9;
const int motorB_1 = 14;
const int motorB_2 = 16;
const int PWM_motorB =10;
//赤外線距離センサ(GP2YOA21)を定義
const int value = A2;
int distance = 0;
//赤外線近接センサ(LM393)を定義
const int LM393_1 = A0;
const int LM393_2 = A1;
int LM393_1value;
int LM393_2value;
//シグナルランプを定義
const int SignalLump = A3;
void setup() {
servo1.attach(3);
servo2.attach(5);
servo3.attach(6);
servo1_min=30,servo1_max=140,servo1_mean=85;
servo2_min=60,servo2_max=100,servo2_mean=80;
servo3_min=60,servo3_max=100,servo3_mean=90;
servo1.write(servo1_mean);
servo2.write(servo2_mean);
servo3.write(servo3_mean);
Serial.begin(9600);
}
void loop(){
for (int i = 0; i < len_1; i++){
servo1.write(position_of_servo1[i], 100, true); //サーボ位置決め
distance = (6787/(analogRead(value)-3))-4;
Serial.print(distance);
Serial.println(“cm”);
LM393_1value = digitalRead(LM393_1);
LM393_2value = digitalRead(LM393_2);
Serial.print (“LM393_1 = “);
Serial.println(LM393_1value);
Serial.print (“LM393_2 = “);
Serial.println(LM393_2value);
if (LM393_1value == 1 || LM393_2value == 0) { //落下または衝突回避のため停止、後進の後、右旋回
return_head();
SignalLump_ONOFF();
stop();
shake_head();
delay(300);
return_head();
updown_head();
delay(300);
return_head();
back(50);
delay(500);
turn_right(50);
delay(600);
}
else if (distance < 10) { //GP2YOA21と障害物との距離が10cm未満の場合、一旦停止少しバック障害物と反対方向に旋回,二回繰り返してもだめなら大きく右旋回
for (int n = 0; n< 2; n++){
if (distance > 85) { //障害物が左の場合
stop();
delay(100);
back(50);
delay(300);
turn_right(50);
delay(600);
}
else { //障害物が右の場合
stop();
delay(100);
back(50);
delay(300);
turn_left(50);
delay(600);
}
turn_right(50);
delay(600);
}
}
else if (distance < 20) { //GP2YOA21と障害物との距離が20cm未満の場合:障害物と反対方向に旋回
if (distance > 85) { //障害物が右の場合
turn_left(50);
delay(300);
}
else { //障害物が左の場合
turn_right(50);
delay(300);
}
}
else { //前方に障害物ない場合は前進
forward(75);
delay(100);
}
}
}
void forward(int x) {
Serial.println(“forward”);
digitalWrite(SignalLump, HIGH);
analogWrite(PWM_motorA, x);
analogWrite(PWM_motorB, x);
digitalWrite(motorA_1, HIGH);
digitalWrite(motorA_2, LOW);
digitalWrite(motorB_1, HIGH);
digitalWrite(motorB_2, LOW);
}
void turn_right(int x){
Serial.println(“turn right”);
digitalWrite(SignalLump, LOW);
analogWrite(PWM_motorA, x);
analogWrite(PWM_motorB, x);
digitalWrite(motorA_1, LOW);
digitalWrite(motorA_2, HIGH);
digitalWrite(motorB_1, HIGH);
digitalWrite(motorB_2, LOW);
delay(300);
}
void turn_left(int x){
Serial.println(“turn left”);
digitalWrite(SignalLump, LOW);
analogWrite(PWM_motorA, x);
analogWrite(PWM_motorB, x);
digitalWrite(motorA_1, HIGH);
digitalWrite(motorA_2, LOW);
digitalWrite(motorB_1, LOW);
digitalWrite(motorB_2, HIGH);
delay(300);
}
void stop(){
Serial.println(“stop”);
digitalWrite(SignalLump, HIGH);
digitalWrite(motorA_1, LOW);
digitalWrite(motorA_2, LOW);
digitalWrite(motorB_1, LOW);
digitalWrite(motorB_2, LOW);
}
void back(int x){
Serial.println(“back”);
digitalWrite(SignalLump, HIGH);
analogWrite(PWM_motorA, x);
analogWrite(PWM_motorB, x);
digitalWrite(motorA_1, LOW);
digitalWrite(motorA_2, HIGH);
digitalWrite(motorB_1, LOW);
digitalWrite(motorB_2, HIGH);
delay(300);
}
void shake_head(){
for (int l = 0; l < len_2; l++){
servo2.write(position_of_servo2[l], 100, true);
delay(300);
}
}
void updown_head(){
for (int m = 0; m < len_3; m++){
servo3.write(position_of_servo3[m], 100, true);
delay(300);
}
}
void return_head(){
servo1.write(servo1_mean);
servo2.write(servo2_mean);
servo3.write(servo3_mean);
}
void SignalLump_ONOFF() {
if(millis() % 500 > 250) {
analogWrite(SignalLump, 255);
}
else {
analogWrite(SignalLump, 0);
}
}