Академический Документы
Профессиональный Документы
Культура Документы
#include <Servo.h>
const int servo1 = 11; // first servo
Servo myservo1; // create servo object to control a servo
const int esc = 12; // first servo
Servo myesc; // create servo object to control a servo
const int buttonPin = 13; // the number of the pushbutton pin
int buttonState = 0;
const int ledpin = 2;
char data = 0;
int driverState = 0;
int servoval = 100;
#define trigD1 9
#define echoD1 8
#define trigD2 7
#define echoD2 6
#define trigD3 5
#define echoD3 4
#define trigD4 3
#define echoD4 10
void setup() {
// put your setup code here, to run once:
void loop() {
SonarSensor(trigD1, echoD1);
distance1 = distance;
SonarSensor(trigD2, echoD2);
distance2 = distance;
SonarSensor(trigD3, echoD3);
distance3 = distance;
SonarSensor(trigD4, echoD4);
distance4 = distance;
difference = (distance1-distance2);
absDiff = abs(difference);
buttonState = digitalRead(buttonPin);
if(data == '1')
{
driverState = 1;
}
if(data == '0')
{
driverState = 0;
}
}
if(driverState == 0)
{
myesc.write(80);
myservo1.write(100);
digitalWrite(ledpin, LOW);
}
if(driverState == 1)
{
//Drive code here
//variables: distance1, distance2, distance3 in cm
digitalWrite(ledpin, HIGH);
else
{
if((distance1 == 0) || (distance1 > 45))
{
}
else
{
myesc.write(91);
myservo1.write(130);
}
}
}
}
Serial.print(distance1);
Serial.print("\n \n distance2 \n");
Serial.print(distance2);
Serial.print("\n \n distance3 \n");
Serial.print(distance3);
Serial.print("\n \n distance1 \n");
//Serial.print(difference);
//Serial.print("\n \n \n");