|
|
#include <Servo.h>
Servo servo_10;
int MotorLeft1=14; //A0 IN1
int MotorLeft2=15; //A1 IN2
int MotorLeftPWM=5; //PWM5
int MotorRight1=16; //A2 IN3
int MotorRight2=17; //A3 IN4
int MotorRightPWM=3; //PWM5
const int SensorRight = 12;
const int SensorLeft=13;
const int CardWidth=16;
const int CardLength=20;
const int Left45=350;
const int Left90=625;
const int Right45=350;
const int Right90=625;
int distance_0,distance_45,distance_90,distance_135,distance_180;
int SensorRightStatus=1;
int SensorLeftStatus=1;
int maxdistance=0;
int tmpdistance=0;
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
servo_10.attach(10);
pinMode(MotorLeft1, OUTPUT); // 腳位 14
pinMode(MotorLeft2, OUTPUT); // 腳位 15
pinMode(MotorRight1, OUTPUT); // 腳位 16
pinMode(MotorRight2, OUTPUT); // 腳位 17
pinMode(MotorRightPWM, OUTPUT); // 腳位 3 (PWM)
pinMode(MotorLeftPWM, OUTPUT); // 腳位 5 (PWM)
pinMode(SensorLeft, INPUT); //定義左感測(cè)器
pinMode(SensorRight, INPUT); //定義右感測(cè)器
pinMode(8, OUTPUT);
pinMode(9, INPUT);
}
void GoAhead(){
digitalWrite(MotorRight1,HIGH);//IN1 左電機(jī) 高電平反轉(zhuǎn)
digitalWrite(MotorRight2,LOW);//IN2 左電機(jī) 高電平正轉(zhuǎn)
analogWrite(MotorRightPWM,99); //0---100--250
digitalWrite(MotorLeft1,LOW);//IN3 右電機(jī) 高電平正轉(zhuǎn)
digitalWrite(MotorLeft2,HIGH);//IN4 右電機(jī) 高電平反轉(zhuǎn)
analogWrite(MotorLeftPWM,110);
}
void GoBack(){
digitalWrite(MotorRight1,LOW);//IN1 左電機(jī) 高電平反轉(zhuǎn)
digitalWrite(MotorRight2,HIGH);//IN2 左電機(jī) 高電平正轉(zhuǎn)
analogWrite(MotorRightPWM,99); //0---100--250
digitalWrite(MotorLeft1,HIGH);//IN3 右電機(jī) 高電平正轉(zhuǎn)
digitalWrite(MotorLeft2,LOW);//IN4 右電機(jī) 高電平反轉(zhuǎn)
analogWrite(MotorLeftPWM,110);
}
void TurnLeft(int millseconds){
digitalWrite(MotorRight1,HIGH);//IN1 左電機(jī) 高電平反轉(zhuǎn)
digitalWrite(MotorRight2,LOW);//IN2 左電機(jī) 高電平正轉(zhuǎn)
analogWrite(MotorRightPWM,120); //0---100--250
digitalWrite(MotorLeft1,LOW);//IN3 右電機(jī) 高電平正轉(zhuǎn)
digitalWrite(MotorLeft2,LOW);//IN4 右電機(jī) 高電平反轉(zhuǎn)
analogWrite(MotorLeftPWM,0);
delay(millseconds);
}
void TurnRight(int millseconds){
digitalWrite(MotorRight1,LOW);//IN1 左電機(jī) 高電平反轉(zhuǎn)
digitalWrite(MotorRight2,LOW);//IN2 左電機(jī) 高電平正轉(zhuǎn)
analogWrite(MotorRightPWM,0); //0---100--250
digitalWrite(MotorLeft1,LOW);//IN3 右電機(jī) 高電平正轉(zhuǎn)
digitalWrite(MotorLeft2,HIGH);//IN4 右電機(jī) 高電平反轉(zhuǎn)
analogWrite(MotorLeftPWM,143);
delay(millseconds);
}
void Stop(){
digitalWrite(MotorRight1,LOW);//IN1 左電機(jī) 高電平反轉(zhuǎn)
digitalWrite(MotorRight2,LOW);//IN2 左電機(jī) 高電平正轉(zhuǎn)
analogWrite(MotorRightPWM,0); //0---100--250
digitalWrite(MotorLeft1,LOW);//IN3 右電機(jī) 高電平正轉(zhuǎn)
digitalWrite(MotorLeft2,LOW);//IN4 右電機(jī) 高電平反轉(zhuǎn)
analogWrite(MotorLeftPWM,0);
}
float checkdistance_8_9() {
digitalWrite(8, LOW);
delayMicroseconds(2);
digitalWrite(8, HIGH);
delayMicroseconds(10);
digitalWrite(8, LOW);
float distance = pulseIn(9, HIGH) / 58.00;
delay(10);
return distance;
}
void CheckAheadDistance(){
Stop();
servo_10.write(90);
delay(500);
distance_90=checkdistance_8_9();
}
void CheckLeftDistance(){
Stop();
servo_10.write(180);
delay(500);
distance_180=checkdistance_8_9();
}
void CheckRightDistance(){
Stop();
servo_10.write(0);
delay(500);
distance_0=checkdistance_8_9();
}
void CheckAround(){
Stop();
servo_10.write(0);
delay(500);
distance_0=checkdistance_8_9();
servo_10.write(45);
delay(500);
distance_45=checkdistance_8_9();
servo_10.write(90);
delay(500);
distance_90=checkdistance_8_9();
servo_10.write(135);
delay(500);
distance_135=checkdistance_8_9();
servo_10.write(180);
delay(500);
distance_180=checkdistance_8_9();
}
//Track Right Run
const int diff=3;
void loop() {
CheckRightDistance();
while(1){
SensorLeftStatus = digitalRead(SensorLeft);
SensorRightStatus = digitalRead(SensorRight);
if(SensorLeftStatus && SensorRightStatus){
if(distance_0<10){
TurnLeft(1);
}else if(distance_0>12){
TurnRight(1);
}else{
GoAhead();
}
}else{
CheckAheadDistance();
servo_10.write(0);
if(distance_90<CardWidth || !SensorLeftStatus || !SensorRightStatus){
GoBack();
delay(300);
}
TurnLeft(Left90);
}
distance_0=checkdistance_8_9();
}
}
//Base on far distance
/*
void loop() {
CheckAheadDistance();
SensorLeftStatus = digitalRead(SensorLeft);
SensorRightStatus = digitalRead(SensorRight);
while(distance_90>CardWidth+4 && SensorLeftStatus && SensorRightStatus){
GoAhead();
distance_90=checkdistance_8_9();
SensorLeftStatus = digitalRead(SensorLeft);
SensorRightStatus = digitalRead(SensorRight);
}
CheckAround();
SensorLeftStatus = digitalRead(SensorLeft);
SensorRightStatus = digitalRead(SensorRight);
tmpdistance = (CardWidth+4)/2;
if(distance_45<=tmpdistance || distance_90<=tmpdistance || distance_135<=tmpdistance || !SensorLeftStatus || !SensorRightStatus)
{
GoBack();
delay(300);
}
maxdistance=max(distance_0,distance_45);
maxdistance=max(maxdistance,distance_90);
maxdistance=max(maxdistance,distance_135);
maxdistance=max(maxdistance,distance_180);
if(maxdistance==distance_0 || !SensorLeftStatus){
TurnRight(Right90);
}else if(maxdistance==distance_45 || !SensorLeftStatus){
TurnRight(Right45);
}else if(maxdistance==distance_135 || !SensorRightStatus){
TurnLeft(Left45);
}else if(maxdistance==distance_180 || !SensorRightStatus){
TurnLeft(Left90);
}else{
GoBack();
delay(300);
}
}*/
/*
int action=0; //0-go ahead, 1-go back, 2-turn left, 3-turn right
int pre_distance_90;
void loop() {
// put your main code here, to run repeatedly:
//Start, Appouch Right
CheckAheadDistance();
while(distance_90>CardWidth+4){
action=0;
GoAhead();
distance_90=checkdistance_8_9();
}
CheckLeftDistance();
if(distance_180>=CardWidth+4){
CheckAround();
Serial.print("Ahead:");
Serial.println(distance_90);
if(distance_90<=(CardWidth+4)/2){
pre_distance_90=0;
while((distance_90<=CardWidth+4 && pre_distance_90 != distance_90) || distance_45<(CardWidth+4)/2 || distance_135<(CardWidth+4)/2 ) {
Serial.println("GoBack");
action = 1;
GoBack();
pre_distance_90 = distance_90;
delay(100);
distance_90=checkdistance_8_9();
}
}
do{
Serial.println("TrunLeft");
TurnLeft();
delay(100);
distance_90=checkdistance_8_9();
}while(distance_90<=CardWidth+4 && distance_90>(CardWidth+4)/2);
}else{
Serial.println("GoBack");
while(distance_180<CardWidth+4){
//Serial.println("GoBack");
GoBack();
distance_180=checkdistance_8_9();
}
TurnLeft();
delay(100);
CheckAround();
while(distance_90<distance_45){
TurnLeft();
delay(100);
CheckAround();
}
}
}*/
|
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
