树莓派小车倒车入库

树莓派小车倒车入库

引脚初始化：

=void main(){
  //wiringPi初始化
  wiringPiSetup();
  //初始化电机驱动IO口为输出方式
  pinMode(Left_motor_go, OUTPUT);
  pinMode(Left_motor_back, OUTPUT);
  pinMode(Right_motor_go, OUTPUT);
  pinMode(Right_motor_back, OUTPUT);
  //创建两个软件控制的PWM脚
  softPwmCreate(Left_motor_pwm,0,255);
  softPwmCreate(Right_motor_pwm,0,255);
  //定义四路循迹红外传感器为输入接口
  pinMode(TrackSensorLeftPin1, INPUT);
  pinMode(TrackSensorLeftPin2, INPUT);
  pinMode(TrackSensorRightPin1, INPUT);
  pinMode(TrackSensorRightPin2, INPUT);
  //初始化超声波引脚
  pinMode(EchoPin, INPUT);    //定义超声波输入脚
  pinMode(TrigPin, OUTPUT);   //定义超声波输出脚
  //舵机设置为输出模式
  pinMode(ServoPin, OUTPUT);
  //初始化RGB三色LED的IO口为输出方式
  pinMode(LED_R, OUTPUT);
  pinMode(LED_G, OUTPUT);
  pinMode(LED_B, OUTPUT);
  //初始化蜂鸣器IO为输出方式
  pinMode(buzzer, OUTPUT);
  digitalWrite(buzzer, HIGH)

询迹模块

//四路循迹引脚电平状态
	// 0 0 X 0		1 0 X 0		0 1 X 0
	//以上6种电平状态时小车原地右转，速度为250,延时80ms
	//处理右锐角和右直角的转动
	else if ((TrackSensorLeftValue1 == LOW || TrackSensorLeftValue2 == LOW) && TrackSensorRightValue2 == LOW)
	{
		spin_right(200, 200);//右旋
		delay(80);
	}
	//四路循迹引脚电平状态
	// 0 X 0 0		 0 X 0 1		0 X 1 0
	//处理左锐角和左直角的转动
	else if (TrackSensorLeftValue1 == LOW && (TrackSensorRightValue1 == LOW || TrackSensorRightValue2 == LOW))
	{
		spin_left(200, 200); //左旋
		delay(80);
	}
// 0 X X X
	//最左边检测到
	else if (TrackSensorLeftValue1 == LOW)
	{
		spin_left(150, 150);
	}
                 // X X X 0
	//最右边检测到
	else if (TrackSensorRightValue2 == LOW)
	{
		spin_right(150, 150);
	}
	//四路循迹引脚电平状态
	// X 0 1 X
	//处理左小弯
	else if (TrackSensorLeftValue2 == LOW && TrackSensorRightValue1 == HIGH)
	{
		left(0, 200);
	}
//四路循迹引脚电平状态
// X 1 0 X
//处理右小弯
else if (TrackSensorLeftValue2 == HIGH && TrackSensorRightValue1 == LOW)
{
	right(200, 0);
}
//四路循迹引脚电平状态
// X 0 0 X
//处理直线
else if (TrackSensorLeftValue2 == LOW && TrackSensorRightValue1 == LOW)
{
	run(80, 80);
}
//当为1 1 1 1时小车保持上一个小车运行状态

库前停车，寻找车位模块

// 引脚电平状态  0 0 0 0
// 四个灯均检测到
if(TrackSensorLeftValue1 == LOW && TrackSensorLeftValue2 == LOW && TrackSensorRightValue1 == LOW && TrackSensorRightValue2 == LOW)
{
     brake(10);
     spin_left(250,250);
     delay(50);
     //开始寻找车位
     findblack();
     //寻找有无空位;
     if(findparking())
     {
        whistle();//找到车位
        return;
     }
     //开始寻找车位
    findblack();
	//寻找有无空位;
	if(findparking())
	{
       whistle();
       return;
	}
	//没有车位啦
 	servo_control_color();
	//彩灯摇头闪烁
	return;
}
//直行寻找双黑线
void findblack()
{
    while(1)
    {
        //检测到黑线时循迹模块相应的指示灯亮，端口电平为LOW
		//未检测到黑线时循迹模块相应的指示灯灭，端口电平为HIGH
		TrackSensorLeftValue1 = digitalRead(TrackSensorLeftPin1);
		TrackSensorLeftValue2 = digitalRead(TrackSensorLeftPin2);
		TrackSensorRightValue1 = digitalRead(TrackSensorRightPin1);
		TrackSensorRightValue2 = digitalRead(TrackSensorRightPin2);
        //直行直到双黑线
        run(80,80);
        delay(200);
        //找到双黑线
        if(TrackSensorLeftValue1 == LOW && TrackSensorLeftValue2 == LOW && TrackSensorRightValue1 == LOW && TrackSensorRightValue2 == LOW)
        {
            brake(20);
            corlor_led(ON, OFF, OFF);//点亮红灯
            return;
        }
    }
}
//超声波左右探测有无空车位
int findparking()
{
    //超声波探头左右寻找是否有空车位
    //向左侧探寻有无空位
    servo_appointed_detection(180);
    float distance = Distance_test();
    if (distance > 100) //有空位
    {
        corlor_led(OFF, ON, OFF);//点亮绿灯
        servo_appointed_detection(90);

        spin_right(250,250);
        delay(400);
        back(60);

        return 1;
    }
//向右侧探寻有无空位
    servo_appointed_detection(0);
    distance = Distance_test();
    if (distance > 100) //有空位
    {
        corlor_led(OFF, ON, OFF);//点亮绿灯
        servo_appointed_detection(90);
        spin_left(250,250);
        delay(400);
        back(60);
        return 1;
    }

    servo_appointed_detection(90);
    return 0;
}