久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

標題: 避障小車代碼分享 [打印本頁]
作者: SandyLee8888    時間: 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();
    }

  }

}*/






歡迎光臨 (http://www.zg4o1577.cn/bbs/) Powered by Discuz! X3.1
主站蜘蛛池模板： 亚洲国产精品一区二区久久 | 久久久黄色 | 老外几下就让我高潮了 | 久草视频观看 | 亚洲视频在线一区 | 欧美激情网站 | 亚洲一区二区三区在线 | 九色国产 | 激情网五月天 | 国产成人在线播放 | 久久精品亚洲精品国产欧美 | 91精品国产一区二区三区蜜臀 | 国产精品久久久久久久久免费丝袜 | 久久精品久久久久久 | 伊人手机在线视频 | 久久91av| 欧美一级片在线观看 | 乱一性一乱一交一视频a∨ 色爱av | 成人高清网站 | 天天天操操操 | 最近日韩中文字幕 | 日韩精品一区二区三区视频播放 | 国产精品久久一区二区三区 | 一级黄片一级毛片 | 综合自拍| 一区二区av在线 | 亚洲精品一区二区三区蜜桃久 | 亚洲精品久久久久久国产精华液 | 亚洲成av人片在线观看 | 亚洲精品美女 | 欧美天堂 | 亚洲成人在线视频播放 | 青青99| 亚洲最大的黄色网址 | 看片天堂| 国产精品视频一区二区三区不卡 | 欧美精品一区在线 | 国内自拍第一页 | 毛片一区二区三区 | 成人欧美一区二区三区黑人孕妇 | 日韩精品在线观看免费 |