Merge branch 'ul_wave' into 'main'

Ul wave

See merge request !2

ul_wave.c

+#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