避障小车代码分享

ID:272164 · 发表于 2018-1-5 23:33

#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); //定義左感測器
  pinMode(SensorRight, INPUT); //定義右感測器

  pinMode(8, OUTPUT);
  pinMode(9, INPUT);

}

void GoAhead(){
digitalWrite(MotorRight1,HIGH);//IN1 左电机高电平反转
digitalWrite(MotorRight2,LOW);//IN2 左电机高电平正转
analogWrite(MotorRightPWM,99); //0---100--250

digitalWrite(MotorLeft1,LOW);//IN3 右电机高电平正转
digitalWrite(MotorLeft2,HIGH);//IN4 右电机高电平反转
analogWrite(MotorLeftPWM,110);
}

void GoBack(){
digitalWrite(MotorRight1,LOW);//IN1 左电机高电平反转
digitalWrite(MotorRight2,HIGH);//IN2 左电机高电平正转
analogWrite(MotorRightPWM,99); //0---100--250

digitalWrite(MotorLeft1,HIGH);//IN3 右电机高电平正转
digitalWrite(MotorLeft2,LOW);//IN4 右电机高电平反转
analogWrite(MotorLeftPWM,110);
}

void TurnLeft(int millseconds){
digitalWrite(MotorRight1,HIGH);//IN1 左电机高电平反转
digitalWrite(MotorRight2,LOW);//IN2 左电机高电平正转
analogWrite(MotorRightPWM,120); //0---100--250

digitalWrite(MotorLeft1,LOW);//IN3 右电机高电平正转
digitalWrite(MotorLeft2,LOW);//IN4 右电机高电平反转
analogWrite(MotorLeftPWM,0);
delay(millseconds);
}

void TurnRight(int millseconds){
digitalWrite(MotorRight1,LOW);//IN1 左电机高电平反转
digitalWrite(MotorRight2,LOW);//IN2 左电机高电平正转
analogWrite(MotorRightPWM,0); //0---100--250

digitalWrite(MotorLeft1,LOW);//IN3 右电机高电平正转
digitalWrite(MotorLeft2,HIGH);//IN4 右电机高电平反转
analogWrite(MotorLeftPWM,143);
delay(millseconds);
}

void Stop(){
digitalWrite(MotorRight1,LOW);//IN1 左电机高电平反转
digitalWrite(MotorRight2,LOW);//IN2 左电机高电平正转
analogWrite(MotorRightPWM,0); //0---100--250

digitalWrite(MotorLeft1,LOW);//IN3 右电机高电平正转
digitalWrite(MotorLeft2,LOW);//IN4 右电机高电平反转
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();
}

  }

}*/

帐号		自动登录	找回密码
密码			立即注册