如何制作一個智能尋路+避障的小車？

05作品源碼

#include
#include
#include
#include 
#define EXPORT_GPIO1 "echo 2027 > /sys/class/gpio/export"
#define EXPORT_GPIO2 "echo 2020 > /sys/class/gpio/export"
#define EXPORT_GPIO3 "echo 2021 > /sys/class/gpio/export"
#define EXPORT_GPIO4 "echo 2022 > /sys/class/gpio/export"

#define UEXPORT_GPIO1 "echo 2027 > /sys/class/gpio/unexport"
#define UEXPORT_GPIO2 "echo 2020 > /sys/class/gpio/unexport"
#define UEXPORT_GPIO3 "echo 2021 > /sys/class/gpio/unexport"
#define UEXPORT_GPIO4 "echo 2022 > /sys/class/gpio/unexport"

#define GPIO1_OUTPUT "echo out > /sys/class/gpio/gpio2027/direction"
#define GPIO2_OUTPUT "echo out > /sys/class/gpio/gpio2020/direction"
#define GPIO3_OUTPUT "echo out > /sys/class/gpio/gpio2021/direction"
#define GPIO4_OUTPUT "echo out > /sys/class/gpio/gpio2022/direction"

#define LOWGPIO1 "echo 0 > /sys/class/gpio/gpio2027/value"
#define LOWGPIO2 "echo 0 > /sys/class/gpio/gpio2020/value"
#define LOWGPIO3 "echo 0 > /sys/class/gpio/gpio2021/value"
#define LOWGPIO4 "echo 0 > /sys/class/gpio/gpio2022/value"

#define HIGHGPIO1 "echo 1 > /sys/class/gpio/gpio2027/value"
#define HIGHGPIO2 "echo 1 > /sys/class/gpio/gpio2020/value"
#define HIGHGPIO3 "echo 1 > /sys/class/gpio/gpio2021/value"
#define HIGHGPIO4 "echo 1 > /sys/class/gpio/gpio2022/value"

//紅外
#define EXPORT_HW1 "echo 2023 > /sys/class/gpio/export"
#define EXPORT_HW2 "echo 2024 > /sys/class/gpio/export"
#define EXPORT_HW3 "echo 2025 > /sys/class/gpio/export"
#define EXPORT_HW4 "echo 2026 > /sys/class/gpio/export"

#define HW1_INPUT "echo in > /sys/class/gpio/gpio2023/direction"
#define HW2_INPUT "echo in > /sys/class/gpio/gpio2024/direction"
#define HW3_INPUT "echo in > /sys/class/gpio/gpio2025/direction"
#define HW4_INPUT "echo in > /sys/class/gpio/gpio2026/direction"

#define UNEXPORT_HW1 "echo 2023 > /sys/class/gpio/unexport"
#define UNEXPORT_HW2 "echo 2024 > /sys/class/gpio/unexport"
#define UNEXPORT_HW3 "echo 2025 > /sys/class/gpio/unexport"
#define UNEXPORT_HW4 "echo 2026 > /sys/class/gpio/unexport"

#define run 1
#define left 3
#define right 4
#define back 2
#define stop 5

void INIT()
{

system(EXPORT_GPIO1);
system(EXPORT_GPIO2);
system(EXPORT_GPIO3);
system(EXPORT_GPIO4);

system(EXPORT_HW1);
system(EXPORT_HW2);
system(EXPORT_HW3);
system(EXPORT_HW4);

system(GPIO1_OUTPUT);
system(GPIO2_OUTPUT);
system(GPIO3_OUTPUT);
system(GPIO4_OUTPUT);

system(HW1_INPUT);
system(HW2_INPUT);
system(HW3_INPUT);
system(HW4_INPUT);
//printf("1
");

}

void RUN()
{
system(LOWGPIO1);//0
system(HIGHGPIO2);//1
system(HIGHGPIO3);//1
system(LOWGPIO4);//0
//printf("2
");

}
void STOP()
{
system(LOWGPIO1);
system(LOWGPIO2);
system(LOWGPIO3);
system(LOWGPIO4);
//printf("3
");
}
void LEFT()
{
system(LOWGPIO1);//0
system(HIGHGPIO2);//1
system(HIGHGPIO3);//1
system(HIGHGPIO4);//1
}
void RIGHT()
{
system(HIGHGPIO1);//1
system(HIGHGPIO2);//1
system(HIGHGPIO3);//1
system(LOWGPIO4);//0
}
void BACK()
{
system(HIGHGPIO1);//1
system(LOWGPIO2);//0
system(LOWGPIO3);//0
system(HIGHGPIO4);//1
}
void MOTORRUN(int cmd)
{
  switch(cmd)
  {
  case 1:
  RUN();
  break;
  case 2:
  BACK();
  break;
  case 3:
  LEFT();
  break;
  case 4:
  RIGHT();
  break;
  case 5:
  STOP();
  break;
  }
}
int GETHW(int gpio)
{

    char path[64];
    char value_str[3];
    int fd;

    snprintf(path, sizeof(path), "/sys/class/gpio/gpio%d/value", gpio);
    fd = open(path, O_RDONLY);
    if (fd < 0) {
        perror("Failed to open gpio value for reading!");
        return -1;
    }

    if (read(fd, value_str, 3) < 0) {
        perror("Failed to read value!");
        return -1;
    }

    close(fd);
    return (atoi(value_str));
}

void TRACE()
{
int l1=GETHW(2023);
int l2=GETHW(2024);
int r1=GETHW(2025);
int r2=GETHW(2026);
//printf("%d,%d,%d,%d",l1,l2,r1,r2);
if(l1&&l2&&r1&&r2){
MOTORRUN(run);
printf("直走
");
}
else if(l1+l2r1+r2){
//MOTORRUN(back);
printf("左轉
");
sleep(0.5);
MOTORRUN(left);
}
else if(!r1&&!r2&&!l1&&!l2){
            printf("停止
");
MOTORRUN(stop);
}

}

int main(void)
{
INIT();
while(1)
{
 TRACE();
// printf("2022");
// sleep(1); 
// RUN();
 sleep(0.5);
// STOP();
 }return 0;

}

審核編輯：湯梓紅