From 395041f8e841fd843f057eb6d4f5ac747685c64a Mon Sep 17 00:00:00 2001
From: Moon Dahyun <dahyun723@ajou.ac.kr>
Date: Fri, 23 Jun 2023 22:27:56 +0900
Subject: [PATCH] feat:ul_wave
---
ul_wave.c | 239 ++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 239 insertions(+)
create mode 100644 ul_wave.c
diff --git a/ul_wave.c b/ul_wave.c
new file mode 100644
index 0000000..3a2f6b0
--- /dev/null
+++ b/ul_wave.c
@@ -0,0 +1,239 @@
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <time.h>
+#include <wiringPi.h>
+#include <softPwm.h>
+
+#define BUFFER_MAX 3
+#define DIRECTION_MAX 35
+#define VALUE_MAX 30
+
+#define IN 0
+#define OUT 1
+#define LOW 0
+#define HIGH 1
+
+#define POUT 23
+#define PIN 24
+
+#define POUT2 17
+#define PIN2 6
+
+#define SUBMOTOR 26
+
+
+int timeout = 3000000;
+
+static int GPIOExport(int pin) {
+
+ char buffer[BUFFER_MAX];
+ ssize_t bytes_written;
+ int fd;
+
+ fd=open("/sys/class/gpio/export", O_WRONLY);
+ if(-1 == fd) {
+ fprintf(stderr,"Failed to open export for writing!\n");
+ return(-1);
+ }
+
+ bytes_written=snprintf(buffer,BUFFER_MAX,"%d",pin);
+ write(fd,buffer,bytes_written);
+ close(fd);
+ return(0);
+}
+
+static int GPIOUnexport(int pin) {
+ char buffer[BUFFER_MAX];
+ ssize_t bytes_written;
+ int fd;
+
+ fd=open("/sys/class/gpio/unexport", O_WRONLY);
+ if(-1==fd) {
+ fprintf(stderr,"Failed to open unexport for writing!\n");
+ return(-1);
+ }
+ bytes_written=snprintf(buffer,BUFFER_MAX,"%d",pin);
+ write(fd, buffer,bytes_written);
+ close(fd);
+ return(0);
+}
+
+static int GPIODirection(int pin, int dir) {
+ static const char s_directions_str[] = "in\0out";
+ char path[DIRECTION_MAX]="/sys/class/gpio/gpio%d/direction";
+ int fd;
+
+ snprintf(path,DIRECTION_MAX,"/sys/class/gpio/gpio%d/direction",pin);
+
+ fd=open(path, O_WRONLY);
+ if(-1==fd) {
+ fprintf(stderr, "Failed to open gpio direction for writing!\n");
+ return(-1);
+ }
+
+ if(-1 == write(fd,&s_directions_str[IN==dir?0:3], IN==dir?2:3)) {
+ fprintf(stderr,"Failed to set direction!\n");
+ return(-1);
+ }
+ close(fd);
+ return(0);
+}
+
+static int GPIORead(int pin) {
+ char path[VALUE_MAX];
+ char value_str[3];
+ int fd;
+
+ snprintf(path,VALUE_MAX,"/sys/class/gpio/gpio%d/value",pin);
+ fd=open(path,O_RDONLY);
+ if(-1==fd) {
+ fprintf(stderr,"Failed to open gpio value for reading!\n");
+ return(-1);
+ }
+ if(-1==read(fd,value_str,3)) {
+ fprintf(stderr,"Failed to read value!\n");
+ return(-1);
+
+ }
+
+ close(fd);
+
+ return(atoi(value_str));
+}
+
+static int GPIOWrite(int pin, int value) {
+ static const char s_value_str[]="01";
+ char path[VALUE_MAX];
+ int fd;
+
+ snprintf(path,VALUE_MAX,"/sys/class/gpio/gpio%d/value",pin);
+ fd=open(path,O_WRONLY);
+ if(-1==fd) {
+ fprintf(stderr,"Failed to open gpio value for writing!\n");
+ return(-1);
+ }
+ if(1 != write(fd, &s_value_str[LOW==value ? 0: 1],1)) {
+ fprintf(stderr,"Failed to write value!\n");
+ close(fd); //추가구문
+ return(-1);
+ }
+
+ close(fd);
+ return(0);
+
+
+}
+
+int main(int argc, char *argv[]) {
+ int repeat = 9;
+ clock_t start_t,end_t;
+ clock_t start_t2,end_t2;
+ double time=0;
+ double time2=0;
+
+ wiringPiSetupGpio();
+
+ softPwmCreate(SUBMOTOR, 0, 200);
+
+ if(-1== GPIOExport(POUT) || -1 == GPIOExport(PIN) || -1 == GPIOExport(POUT2) || -1 == GPIOExport(PIN2) || -1 == GPIOExport(SUBMOTOR)) {
+ printf("gpio export err\n");
+ return(1);
+ }
+
+ usleep(100000);
+
+ if(-1 == GPIODirection(POUT, OUT) || -1 == GPIODirection(PIN,IN) || -1 == GPIODirection(POUT2,OUT) || -1 == GPIODirection(PIN2,IN) || -1 == GPIODirection(SUBMOTOR, OUT)){
+ printf("gpio dirction err\n");
+ return(2);
+ }
+
+ GPIOWrite(POUT,0);
+ GPIOWrite(POUT2,0);
+ GPIOWrite(SUBMOTOR, 0);
+ usleep(100000);
+
+
+do {
+ if(-1 == GPIOWrite(POUT,1) || -1 == GPIOWrite(POUT2,1)) {
+ printf("gpio write/trigger err\n");
+ return(3);
+ }
+
+ usleep(10);
+ GPIOWrite(POUT,0);
+ GPIOWrite(POUT2,0);
+
+ while(GPIORead(PIN) ==0) {
+ start_t = clock();
+ }
+ printf("01");
+
+ while(GPIORead(PIN) ==1 ){
+ end_t = clock();
+
+ }
+ printf("02\n");
+
+ time=(double)(end_t-start_t)/CLOCKS_PER_SEC;
+ printf("timeL %.4lf\n",time);
+ printf("distance: %.2lfcm\n", time *34000 /2);
+
+ while (GPIORead(PIN2) == 0 && timeout > 0) {
+ start_t = clock();
+ timeout -= 1000;
+ // 1밀리초(1000마이크로초)씩 감소
+ usleep(1000);
+ }
+
+ if (timeout <= 0) {
+ printf("Timeout occurred!\n");
+ usleep(100000);
+
+}
+
+ while (GPIORead(PIN2) == 1) {
+ end_t = clock();
+ printf("04");
+
+ }
+ time2 = (double)(end_t - start_t) / CLOCKS_PER_SEC;
+ printf("Time2: %.4lf\n", time2);
+ printf("Distance2: %.2lf cm\n", time2 * 34000 /2);
+
+ // 서보모터 제어
+ if (time < time2) {
+ // 초음파 센서 1이 더 가까운 경우
+ GPIOWrite(SUBMOTOR, 1);
+ usleep(2000000); // 일정 시간 대기 // 서보모터 정지
+
+ softPwmWrite(SUBMOTOR, 10); // 각도 조절, 0 ~ 200 범위
+ usleep(2000000); // 일정 시간 대기
+ softPwmWrite(SUBMOTOR, 0);
+ } else {
+ // 초음파 센서 2가 더 가까운 경우
+
+ softPwmWrite(SUBMOTOR, 20); // 각도 조절, 0 ~ 200 범위
+ usleep(2000000); // 일정 시간 대기
+ softPwmWrite(SUBMOTOR, 0); // 서보모터 정지
+ // GPIOWrite(SUBMOTOR, 1);
+ // usleep(2000000); // 일정 시간 대기
+ // GPIOWrite(SUBMOTOR, 0); // 서보모터 정지
+ }
+
+ usleep(1800000);
+
+
+ }
+ while(repeat—);
+
+ if(-1 == GPIOUnexport(POUT) || -1 == GPIOUnexport(PIN) || -1 == GPIOUnexport(POUT2) || -1 == GPIOUnexport(PIN2) || -1 == GPIOUnexport(SUBMOTOR))
+ return(4);
+
+ printf("complete\n");
+
+ return(0);
+}
\ No newline at end of file
--
GitLab