Merge branch 'master' of...

Merge branch 'master' of https://git.ajou.ac.kr/latteneun_dronesafetysystem/dronesafetysystem_latteneun

README.md

+## 상업용 드론 안정성 인증 시스템 
+
+### 사용 센서 및 액츄에이터
+- 1. Sensor
+    - 초음파 센서 (HC-SR04) : 2 EA
+    - 자이로 센서 (MPU6050) : 1 EA
+- 2. Actuator
+    - 서보모터 (SG90) : 2 EA
+    - RGB : 4 EA
+
+### 구현 시스템
+
+- 1. 충돌회피 시스템  
+- 2. 배송물 평형유지 시스템
+
+#### 충돌회피 시스템
+
+<img src="/uploads/5a1d74c3b12474cac9db5876da56734d/readme_1.PNG" width="65%" height="65%">
+
+- 참고 코드 : w4_client_velocity.c , server.c
+
+#### 배송물 평형유지 시스템
+
+<img src="/uploads/a884968f935932e59fcb92e775064761/readme_2.PNG" width="65%" height="65%">
+
+- 참고 코드 : Gyro.c, server.c
\ No newline at end of file

server.c

+#include <sys/stat.h>
+#include <sys/types.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <pthread.h>
+#include <unistd.h>
+#include <string.h>
+#include <arpa/inet.h>
+#include <sys/socket.h>
+
+#include <time.h>
+
+#define IN 0
+#define OUT 1
+#define LOW 0
+#define HIGH 1
+
+#define R1 4
+#define G1 17
+#define B1 27
+
+#define R2 22
+#define G2 5
+#define B2 6
+
+#define R3 23
+#define G3 24
+#define B3 25
+
+#define R4 16
+#define G4 20
+#define B4 21
+
+#define BUFFER_MAX 45
+#define DIRECTION_MAX 45
+#define VALUE_MAX 256
+char clipara_rgb[6];
+char clipara_motor[6];
+int exitcode=0;
+
+
+static int PWMExport(int pwmnum){
+    char buffer[BUFFER_MAX];
+    int bytes_written;
+    int fd;
+
+    fd = open("/sys/class/pwm/pwmchip0/unexport", O_WRONLY);
+    if(-1 == fd){
+        fprintf(stderr, "Failed to open in unexport!\n");
+        return(-1);
+    }
+
+    bytes_written = snprintf(buffer, BUFFER_MAX, "%d", pwmnum);
+    write(fd, buffer, bytes_written);
+    close(fd);
+
+    sleep(1);
+    fd = open("/sys/class/pwm/pwmchip0/export", O_WRONLY);
+    if(-1 == fd){
+        fprintf(stderr, "Failed to open in export!\n");
+        return(-1);
+    }
+    bytes_written = snprintf(buffer, BUFFER_MAX, "%d", pwmnum);
+    write(fd, buffer, bytes_written);
+    close(fd);
+    sleep(1);
+    return(0);
+}
+
+static int PWMEnable(int pwmnum){
+    static const char s_unenable_str[] = "0";
+    static const char s_enable_str[] = "1";
+
+    char path[DIRECTION_MAX];
+    int fd;
+
+    snprintf(path, DIRECTION_MAX, "/sys/class/pwm/pwmchip0/pwm%d/enable", pwmnum);
+    fd = open(path, O_WRONLY);
+    if(-1 == fd){
+        fprintf(stderr, "Failed to open in enalbe!\n");
+        return(-1);
+    }
+
+    write(fd, s_unenable_str, strlen(s_unenable_str));
+    close(fd);
+
+    fd = open(path, O_WRONLY);
+    if(-1 == fd){
+        fprintf(stderr, "Failed to open in enalbe!\n");
+        return(-1);
+    }
+
+    write(fd, s_enable_str, strlen(s_enable_str));
+    close(fd);
+    return(0);
+}
+
+static int PWMWritePeriod(int pwmnum, int value){
+    char s_values_str[VALUE_MAX];
+    char path[VALUE_MAX];
+    int fd,byte;
+
+    snprintf(path, VALUE_MAX, "/sys/class/pwm/pwmchip0/pwm%d/period", pwmnum);
+    fd = open(path, O_WRONLY);
+    if(-1 == fd){
+        fprintf(stderr, "Failed to open in period!\n");
+        return(-1);
+    }
+
+    byte = snprintf(s_values_str, VALUE_MAX, "%d", value);
+
+    if(-1 == write(fd,s_values_str,byte)){
+        fprintf(stderr, "Failed to write value in period!\n");
+        return(-1);
+    }
+
+    close(fd);
+    return(0);
+}
+
+static int PWMWriteDutyCycle(int pwmnum, int value){
+    char path[VALUE_MAX];
+    char s_values_str[VALUE_MAX];
+    int fd, byte;
+
+    snprintf(path, VALUE_MAX, "/sys/class/pwm/pwmchip0/pwm%d/duty_cycle", pwmnum);
+    fd = open(path, O_WRONLY);
+    if(-1 == fd){
+        fprintf(stderr, "Failed to open in duty_cycle!\n");
+        return(-1);
+    }
+
+    byte = snprintf(s_values_str, VALUE_MAX, "%d", value);
+
+    if(-1 == write(fd, s_values_str, byte)){
+        fprintf(stderr, "Failed to write value! in duty_cycle\n");
+        close(fd);
+        return(-1);
+    }
+
+    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,"falied to pen unexport\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,"%d pin err ",pin);
+      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 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 export wr");
+      return 1;
+   }
+   bytes_written=snprintf(buffer,BUFFER_MAX, "%d", pin);
+   write(fd,buffer,bytes_written);
+   close(fd);
+   return 0;
+}
+
+
+static int GPIOWrite(int pin, int value){
+      static const char s_values_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 open gpio write\n");
+         return -1;
+      }
+      
+      //0 1 selection
+      if(1!=write(fd,&s_values_str[LOW==value ? 0:1],1)){
+         fprintf(stderr,"failed to write value\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 val\n");
+        return -1;
+    }
+    close(fd);
+
+    return(atoi(value_str));
+}
+
+void error_handling(char *message){
+    fputs(message,stderr);
+    fputc('\n',stderr);
+    exit(1);
+}
+
+int unexport_GPIOs(){
+    //Enable GPIO pins
+    if (-1 == GPIOUnexport(R1) || -1 == GPIOUnexport(R2)|| -1 == GPIOUnexport(R3)|| -1 == GPIOUnexport(R4)|| -1 == GPIOUnexport(G1)|| -1 == GPIOUnexport(G2)|| -1 == GPIOUnexport(G3)
+    || -1 == GPIOUnexport(G4)|| -1 == GPIOUnexport(B1)|| -1 == GPIOUnexport(B2)|| -1 == GPIOUnexport(B3)|| -1 == GPIOUnexport(B4))
+        return -1;
+    return 0;
+}
+int export_GPIOs(){
+    //Enable GPIO pins
+    if (-1 == GPIOExport(R1) || -1 == GPIOExport(R2)|| -1 == GPIOExport(R3)|| -1 == GPIOExport(R4)|| -1 == GPIOExport(G1)|| -1 == GPIOExport(G2)|| -1 == GPIOExport(G3)
+    || -1 == GPIOExport(G4)|| -1 == GPIOExport(B1)|| -1 == GPIOExport(B2)|| -1 == GPIOExport(B3)|| -1 == GPIOExport(B4))
+        return -1;
+    return 0;
+}
+int direction_GPIOs(){
+    if(-1==GPIODirection(R1,OUT)||-1==GPIODirection(R2,OUT)||-1==GPIODirection(R3,OUT)||-1==GPIODirection(R4,OUT)||-1==GPIODirection(G1,OUT)||-1==GPIODirection(G2,OUT)
+    ||-1==GPIODirection(G3,OUT)||-1==GPIODirection(G4,OUT)||-1==GPIODirection(B1,OUT)||-1==GPIODirection(B2,OUT)||-1==GPIODirection(B3,OUT)||-1==GPIODirection(B4,OUT))
+        return -1;
+    return 0;
+}
+int turnOffRGBs(){
+    if(-1==GPIOWrite(R1,0)||-1==GPIOWrite(G1,0)||-1==GPIOWrite(B1,0)||-1==GPIOWrite(R2,0)||-1==GPIOWrite(G2,0)||-1==GPIOWrite(B2,0)
+    ||-1==GPIOWrite(R3,0)||-1==GPIOWrite(G3,0)||-1==GPIOWrite(B3,0)||-1==GPIOWrite(R4,0)||-1==GPIOWrite(G4,0)||-1==GPIOWrite(B4,0))
+        return -1;
+    return 0;
+}
+
+int RGB_setting(int position,int r,int g,int b){
+    int Rn,Gn,Bn;
+    if(position==1){
+        Rn=R1; Gn=G1; Bn=B1;
+    }else if(position==2){
+        Rn=R2; Gn=G2; Bn=B2;
+    }else if(position==3){
+        Rn=R3; Gn=G3; Bn=B3;
+    }if(position==4){
+        Rn=R4; Gn=G4; Bn=B4;
+    }
+
+    if(-1==GPIOWrite(Rn,r)||-1==GPIOWrite(Gn,g)||-1==GPIOWrite(Bn,b))
+        return -1;
+    return 0;
+}
+
+int RGB_8steps(int pos,int step){
+    int r,g,b;
+     for (int i = 2; i >= 0; --i) { //8자리 숫자까지 나타냄
+        int result = step >> i & 1;
+        if(i==2)
+            r=result;
+        if(i==1)
+            g=result;
+        if(i==0)
+            b=result;
+    }
+    RGB_setting(pos,r,g,b);
+}
+
+int motor_speed(int step){
+    if(step==0){
+        return 0;
+    }else if(step==1){
+        return 500;
+    }else if(step==2){
+        return 100;
+    }
+}
+    
+    void step3_motor_speed(int pwmnum,int step,int period){
+        double f=0.0;
+         int speed=motor_speed(step);
+         if(step==0){
+            PWMWriteDutyCycle(pwmnum,0);    
+            return;
+         }
+            
+
+        f=0.05;        
+        PWMWriteDutyCycle(pwmnum,period*f);
+        usleep(speed*1000);
+         f=0.990;
+        PWMWriteDutyCycle(pwmnum,period*f);
+        usleep(speed*1000);
+    }
+
+void *motor_thread(){
+fprintf(stderr,"motor thread\n");
+    int period=9000000;
+    PWMExport(0); //pwm0 is gpio18
+    PWMWritePeriod(0,period);
+    PWMWriteDutyCycle(0,0);
+    PWMEnable(0);
+
+    
+    while(1){
+        
+        int d1=clipara_motor[1]-48;
+        
+        // printf("thr 1 motor %d\n",d1);
+        if(d1==-48){
+            usleep(10*1000);
+            continue;
+        }
+        step3_motor_speed(0,d1,period);
+       
+        
+    }
+
+    
+}
+
+void *motor_thread2(){
+    // fprintf(stderr,"motor thread\n");
+    int period=9000000;
+
+    PWMExport(1); //pwm0 is gpio18
+    PWMWritePeriod(1,period);
+    PWMWriteDutyCycle(1,0);
+    PWMEnable(1);
+    
+    int speed=1000;
+    while(1){
+        int d2=clipara_motor[2]-48;
+        // printf("thr 2 motor %d\n",d2);
+        // printf("thr 2 %s",clipara_motor);
+        if(d2==-48){
+            usleep(10*1000);
+            continue;
+        }
+        step3_motor_speed(1,d2,period);
+        
+    }
+}
+
+
+void *rgb_thread(){
+
+    if(-1==export_GPIOs())
+        return 1;
+    //Set GPIO directions
+    if (-1 ==direction_GPIOs())
+        return (2);
+    if (-1 == turnOffRGBs())
+        return (3);
+    
+     int c=0;
+    while(1){
+         if(exitcode==1){
+            turnOffRGBs();
+            break;
+        }
+        int d1=clipara_rgb[1]-48;
+        int d2=clipara_rgb[2]-48;
+        int d3=clipara_rgb[3]-48;
+        int d4=clipara_rgb[4]-48;
+        if(d1==-48&&d1==d2&&d1==d3&&d1==d4){
+            continue;
+        }
+        // printf("clipara_rgb: %s ",clipara_rgb);
+        // printf("%d %d %d %d \n",d1,d2,d3,d4);
+        RGB_8steps(1,d1);
+        RGB_8steps(2,d2);
+        RGB_8steps(3,d3);
+         RGB_8steps(4,d4);
+       
+        usleep(1000*50);
+    }
+
+}
+
+int serv_sock=-1;
+struct sockaddr_in serv_addr;
+
+void *rcv_cli(){
+    int clnt_sock2=-1;
+    struct sockaddr_in clnt_addr2;
+    socklen_t clnt_addr_size;
+    int str_len;
+    char msg2[6];
+    if (clnt_sock2 < 0)
+        {
+            clnt_addr_size = sizeof(clnt_addr2);
+            clnt_sock2 = accept(serv_sock, (struct sockaddr *)&clnt_addr2,
+                            &clnt_addr_size);
+            if (clnt_sock2 == -1)
+                error_handling("accept() error");
+        }
+     while (1)
+    {
+        str_len=read(clnt_sock2,msg2,sizeof(msg2));
+        if(str_len==-1)
+            error_handling("read() error");
+        
+        if(msg2[0]=='s'){
+            strcpy(clipara_rgb,msg2);
+
+        }else{
+            strcpy(clipara_motor,msg2);
+            
+        }
+        printf("received msg2 -> %s\n",msg2);
+        usleep(10 * 1000);
+    }
+    close(clnt_sock2);
+}
+int main(int argc, char *argv[])
+{
+    int state = 1;
+    int prev_state = 1;
+    int light = 0;
+
+    int clnt_sock= -1;
+    struct sockaddr_in  clnt_addr;
+    socklen_t clnt_addr_size;
+    
+    char msg[6];
+    char msg2[6];
+    int str_len;
+    
+    pthread_t p_thread[2];
+    int thr_id;
+    int status;
+
+    char p1[] = "thread_1";
+    char p2[] = "thread_2";
+
+    if(argc==3){
+        exitcode=1;
+        fprintf(stderr,"all stopped\n");
+        return 0;
+    }
+    fprintf(stderr,"%d",argc);
+
+    thr_id = pthread_create(&p_thread[0], NULL, motor_thread, NULL);
+    if(thr_id < 0){
+        perror("thread create error : ");
+        exit(0);
+        usleep(1000);
+    }
+     thr_id = pthread_create(&p_thread[1], NULL, motor_thread2, NULL);
+    if(thr_id < 0){
+        perror("thread create error : ");
+        exit(0);
+        usleep(1000);
+    }
+    thr_id = pthread_create(&p_thread[2], NULL, rgb_thread, NULL);
+    if(thr_id < 0){
+        perror("thread create error : ");
+        exit(0);
+        usleep(1000);
+    }
+
+    if (argc != 2)
+    {
+        printf("Usage : %s <port>\n", argv[0]);
+    }
+
+    serv_sock = socket(PF_INET, SOCK_STREAM, 0);
+    if (serv_sock == -1)
+        error_handling("socket() error");
+
+    memset(&serv_addr, 0, sizeof(serv_addr));
+    serv_addr.sin_family = AF_INET;
+    serv_addr.sin_addr.s_addr = htonl(INADDR_ANY);
+    serv_addr.sin_port = htons(atoi(argv[1]));
+    
+    if (bind(serv_sock, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) == -1)
+        error_handling("bind() error");
+    if (listen(serv_sock, 5) == -1)
+        error_handling("listen() error");
+   
+    if (clnt_sock < 0)
+    {
+        clnt_addr_size = sizeof(clnt_addr);
+        clnt_sock = accept(serv_sock, (struct sockaddr *)&clnt_addr,
+                           &clnt_addr_size);
+        if (clnt_sock == -1)
+            error_handling("accept() error");
+    }
+    
+     thr_id = pthread_create(&p_thread[0], NULL, rcv_cli, NULL);
+    if(thr_id < 0){
+        perror("thread create error : ");
+        exit(0);
+        usleep(1000);
+    }
+    while (1)
+    {
+        str_len=read(clnt_sock,msg,sizeof(msg));
+        if(str_len==-1)
+            error_handling("read() error");
+
+        if(msg[0]=='s'){
+            
+            strcpy(clipara_rgb,msg);
+
+        }else{
+            strcpy(clipara_motor,msg);
+            
+        }
+        printf("received msg -> %s\n",msg);
+        usleep(10 * 1000);
+    }
+    close(clnt_sock);
+    close(serv_sock);
+    // //Disable GPIO pins
+    if (-1==unexport_GPIOs())
+        return (4);
+    return (0);
+}
\ No newline at end of file