用python写ros小车避障源码

超声波自动避障小车

#include <Servo.h>
#define Trig 2                 //引脚控制超声波发出声波
#define Echo 3                 //引脚反应接收到返回声波
#define LIN1 7                 //左侧轮子
#define LIN2 6          
#define RIN1 5                 //右侧轮子
#define RIN2 4          
//#define Steeringpin 9        //舵机控制端口
int S = 0;                     //初始化距离
Servo Steering;                //转向舵机
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);          //设置比特率 
  pinMode(Trig, OUTPUT);
  pinMode(Echo, INPUT);
  pinMode(LIN1, OUTPUT);
  pinMode(LIN2, OUTPUT);
  pinMode(RIN1, OUTPUT);
  pinMode(RIN2, OUTPUT);
//  pinMode(Steeringpin, OUTPUT);
  Steering.attach(9);           //定义舵机所用引脚
  Steering.write(90);           //初始化舵机角度
  delay(2000);                  //开机延时
}
void loop() {
  // put your main code here, to run repeatedly:
  Steering.write(90);
  Range();                      //测距函数
  if(S <= 5)      //距离太近不足以转弯
  {
    Back();                     //执行后退函数
    delay(300);                 //后退持续时间
  }
  else if(S >= 5 && S <= 15)   //距离中等可以转弯
  {
    Turn();                     //执行转弯判断函数
  }
  else                          //距离充足
  {
    Line();                     //执行直行函数
  }
}
void Range()                    //测距函数
{
  digitalWrite(Trig,LOW);       //测距
  delay(2);                     //延时2微秒
  digitalWrite(Trig,HIGH);
  delay(10);
  digitalWrite(Trig,LOW);
  float distance = pulseIn(Echo,HIGH); //读取高电平时间
  S = float( distance * 17 )/1000;     //把值赋给S
  Serial.println(S);            //向串口发送S的值，可以在显示器上显示距离
}
// float distance = pulseIn(Echo,HIGH);
// S = float( distance * 17 )/1000; 
// 将回波时间换算成cm
// 读取一个引脚的脉冲（HIGH或LOW）。例如，如果value是HIGH，pulseIn()会等待引脚变为HIGH，开始计时，再等待引脚变为LOW并停止计时。
// 返回脉冲的长度，单位微秒。如果在指定的时间内无脉冲函数返回。
// 此函数的计时功能由经验决定，长时间的脉冲计时可能会出错。计时范围从10微秒至3分钟。（1秒=1000毫秒=1000000微秒）
// 接收到的高电平的时间（us）* 340m/s / 2 = 接收到高电平的时间（us） * 17000 cm / 1000000 us = 接收到高电平的时间 * 17 / 1000  (cm)
void Turn()                     //转向函数
{
  Lull();                       //停止
  Steering.write(170);          //使舵机转到170度（左侧）
  delay(1000);                  //留时间给舵机转向
  Range();                      //测距
  Steering.write(90);           //使舵机回复原位
  delay(1000);
  if(S >= 15)                   //距离充足
  {
    turnLeft();                 //左转弯后退
  }
  else                          //距离不足
  {
    Steering.write(10);         //使舵机转到10度（右侧）
    delay(1000);
    Range();
    Steering.write(90);         //使舵机回复原位
    delay(1000);
    if(S >= 15)                 //距离充足
    {
      turnRight();              //右转弯后退
    }
    else
    {
      Back();                   //后退
      int x = random(1);        //产生一个随机数使小车随机转向
      if(x == 0)
        turnLeft();
      else
        turnRight();
    }
  }
}
void Back()                 //后退函数（使电机反转）
{
  digitalWrite(LIN1, LOW);
  digitalWrite(LIN2, HIGH);
  digitalWrite(RIN1, HIGH);
  digitalWrite(RIN2, LOW);
}
void Line()                 //直线行驶函数（使电机正转）
{
  digitalWrite(LIN1, HIGH);
  digitalWrite(LIN2, LOW);
  digitalWrite(RIN1, LOW);
  digitalWrite(RIN2, HIGH);
}
void turnLeft()             //小车左转后退
{
  digitalWrite(LIN1, LOW);
  digitalWrite(LIN2, HIGH);
  digitalWrite(RIN1, LOW);
  digitalWrite(RIN2, LOW);
  delay(1000);
}
void turnRight()            //小车右转后退
{
  digitalWrite(LIN1, LOW);
  digitalWrite(LIN2, LOW);
  digitalWrite(RIN1, HIGH);
  digitalWrite(RIN2, LOW);
  delay(1000);
}
void Lull()                 //停止 
{
  digitalWrite(LIN1, LOW);
  digitalWrite(LIN2, LOW);
  digitalWrite(RIN1, LOW);
  digitalWrite(RIN2, LOW);
}

所需器材：

1：小车底盘一个、万向轮一个、测速码盘2个、车轮两个

2：减速直流电机2个

3：紧固件4片、螺丝若干

4：电池1.5V4个

5：L298N电机驱动模块

6;HC-SR04超声波传感器

7:ARDUINO UNO、USB数据线

8：杜邦线若干

9:舵机

连线步骤

成品图