kayak 自走獨木舟(arduino)
#include <SoftwareSerial.h>
#include <Wire.h>
#include <Servo.h>
Servo servo_2;
Servo servo_3;
Servo servo_4;
long f;
long l;
long r;
long m;
long n;
float ultrasonic_distance_5_6() {
digitalWrite(5, LOW);
digitalWrite(6, LOW);
delayMicroseconds(5);
digitalWrite(5, HIGH);
delayMicroseconds(10);
digitalWrite(5, LOW);
unsigned long sonic_duration = pulseIn(6, HIGH);
float distance_cm = (sonic_duration / 2.0) / 29.1;
return distance_cm;
}
void a1() {
servo_2.write(30);
}
void a2() {
servo_2.write(150);
}
void a3() {
servo_2.write(90);
}
void a5() {
servo_4.write(167);
}
void a6() {
servo_4.write(27);
}
void a7() {
servo_4.write(97);
}
void s() {
servo_2.write(90);
servo_4.write(97);
}
void f1() {
a5();
delay(700);
a2();
delay(700);
a6();
delay(700);
a1();
delay(700);
}
void b() {
a6();
delay(700);
a2();
delay(700);
a5();
delay(700);
a1();
delay(700);
}
void l1() {
a5();
delay(700);
a2();
delay(700);
a6();
delay(700);
a3();
delay(700);
}
void r1() {
a6();
delay(700);
a1();
delay(700);
a5();
delay(700);
a3();
delay(700);
}
void setup()
{
servo_2.attach(2);
servo_3.attach(3);
servo_4.attach(4);
pinMode( 5 , OUTPUT);
pinMode( 6 , INPUT);
}
void loop()
{
servo_3.write(37);
delay(120);
r = ultrasonic_distance_5_6( );
servo_3.write(157);
delay(240);
l = ultrasonic_distance_5_6( );
servo_3.write(97);
delay(120);
f = ultrasonic_distance_5_6( );
if (f < 30 || l < 30 || r < 30) {
m = r - l;
if (m < 0) {
r1();
} else {
l1();
}
} else {
f1();
}
}
