diff --git a/Rarp2.cpp b/Rarp2.cpp new file mode 100644 index 0000000000000000000000000000000000000000..001022d3e23c82a7dd99c087283adaac1e2f30f6 --- /dev/null +++ b/Rarp2.cpp @@ -0,0 +1,554 @@ +/* + * See documentation at https://nRF24.github.io/RF24 + * See License information at root directory of this library + * Author: Brendan Doherty (2bndy5) + */ + +/** + * A simple example of sending data from 1 nRF24L01 transceiver to another. + * + * This example was written to be used on 2 devices acting as "nodes". + * Use `ctrl+c` to quit at any time. + */ +#include <ctime> // time() +#include <iostream> // cin, cout, endl +#include <string> // string, getline() +#include <time.h> // CLOCK_MONOTONIC_RAW, timespec, clock_gettime() +#include <RF24/RF24.h> // RF24, RF24_PA_LOW, delay() + +#include<stdio.h> +#include<stdlib.h> +#include<arpa/inet.h> +#include<sys/socket.h> +#include<unistd.h> +#include <sys/stat.h> +#include <sys/types.h> +#include <fcntl.h> + + +#define IN 0 +#define OUT 1 +#define LOW 0 +#define HIGH 1 +#define VALUE_MAX 40 +#define BUFFER_MAX 3 +#define DIRECTION_MAX_P 45 +#define VALUE_MAX_P 256 + +#define PIN 20 +#define POUT2 21 +#define POUT1 18 +#define PWM 0 + +// 서보모터의 최소 각도와 최대 각도 +#define MIN_ANGLE 500000 +#define MAX_ANGLE 2500000 + +using namespace std; + +/****************** Linux ***********************/ +// Radio CE Pin, CSN Pin, SPI Speed +// CE Pin uses GPIO number with BCM and SPIDEV drivers, other platforms use their own pin numbering +// CS Pin addresses the SPI bus number at /dev/spidev<a>.<b> +// ie: RF24 radio(<ce_pin>, <a>*10+<b>); spidev1.0 is 10, spidev1.1 is 11 etc.. +#define CSN_PIN 0 +#ifdef MRAA + #define CE_PIN 15 // GPIO22 +#else + #define CE_PIN 22 +#endif +// Generic: +RF24 radio(17, CSN_PIN); +/****************** Linux (BBB,x86,etc) ***********************/ +// See http://nRF24.github.io/RF24/pages.html for more information on usage +// See http://iotdk.intel.com/docs/master/mraa/ for more information on MRAA +// See https://www.kernel.org/doc/Documentation/spi/spidev for more information on SPIDEV + +// For this example, we'll be using a payload containing +// a single float number that will be incremented +// on every successful transmission +float payload = 0; + +void setRole(); // prototype to set the node's role +void master(); // prototype of the TX node's behavior +void slave(); // prototype of the RX node's behavior + +// custom defined timer for evaluating transmission time in microseconds +struct timespec startTimer, endTimer; +uint32_t getMicros(); // prototype to get elapsed time in microseconds + + +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 +PWMUnexport(int pwmnum) { + char buffer[BUFFER_MAX]; + ssize_t 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); + 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_P]; + int fd; + snprintf(path, DIRECTION_MAX_P, "/sys/class/pwm/pwmchip0/pwm%d/enable", pwmnum); + fd = open(path, O_WRONLY); + if (-1 == fd) { + fprintf(stderr, "Failed to open in enable!\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 enable!\n"); + return -1; + } + + write(fd, s_enable_str, strlen(s_enable_str)); + close(fd); + return(0); + +} + +static int +PWMUnable(int pwmnum) +{ + static const char s_unable_str[] = "0"; + char path[DIRECTION_MAX_P]; + int fd; + + snprintf(path, DIRECTION_MAX_P, "/sys/class/pwm/pwmchip0/pwm%d/enable", pwmnum); + fd = open(path, O_WRONLY); + if (-1 == fd) { + fprintf(stderr, "Failed to open in enable!\n"); + return -1; + } + + write(fd, s_unable_str, strlen(s_unable_str)); + close(fd); + + return(0); + +} + +static int +PWMWritePeriod(int pwmnum, int value) +{ + char s_values_str[VALUE_MAX_P]; + char path[VALUE_MAX_P]; + int fd, byte; + + snprintf(path, VALUE_MAX_P, "/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, 10, "%d", value); + + if (-1 == write(fd, s_values_str, byte)) { + fprintf(stderr, "Failed to write value in period!\n"); + close(fd); + return(-1); + } + + close(fd); + return(0); +} + +static int +PWMWriteDutyCycle(int pwmnum, int value) +{ + char path[VALUE_MAX_P]; + char s_values_str[VALUE_MAX_P]; + int fd, byte; + + snprintf(path, VALUE_MAX_P, "/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, 10, "%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 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 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 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! [ Pin Number : %d]\n", pin); + close(fd); + return(-1); + } + + close(fd); + return(atoi(value_str)); +} + +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 to open gpio value for writing!\n"); + return(-1); + } + + if (1 != write(fd, &s_values_str[LOW == value ? 0 : 1], 1)) { + fprintf(stderr, "Failed to write value!\n"); + close(fd); + return(-1); + } + + close(fd); + return(0); +} + +static int GPIODirection(int pin, int dir) { + static const char s_directions_str[] = "in\0out"; + + #define DIRECTION_MAX 35 + 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 export 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"); + close(fd); + return(-1); + } + + close(fd); + return(0); +} + +void error_handling(char *message){ + fputs(message,stderr); + fputc('\n', stderr); + exit(1); +} + + + +int main(int argc, char** argv) +{ + int angle = 500000; + int direction = 1; // 1: 증가, -1: 감소 + int clnt_sock; + struct sockaddr_in serv_addr; + char msg[2]; + + char on[2]="1"; + int str_len; + int light = 0; + int repeat = 100000000; + // 이전 상태와 현재 상태를 추적하기 위한 변수 + int state = 1; + int prev_state = 1; + + + + // perform hardware check + if (!radio.begin()) { + cout << "radio hardware is not responding!!" << endl; + return 0; // quit now + } + + // to use different addresses on a pair of radios, we need a variable to + // uniquely identify which address this radio will use to transmit + bool radioNumber = 1; // 0 uses address[0] to transmit, 1 uses address[1] to transmit + + // print example's name + cout << argv[0] << endl; + + // Let these addresses be used for the pair + uint8_t address[2][6] = {"1Node", "LIFT1"}; + // It is very helpful to think of an address as a path instead of as + // an identifying device destination + + // Set the radioNumber via the terminal on startup + cout << "Which radio is this? Enter '0' or '1'. Defaults to '0' "; + string input; + getline(cin, input); + radioNumber = input.length() > 0 && (uint8_t)input[0] == 49; + + // save on transmission time by setting the radio to only transmit the + // number of bytes we need to transmit a float + radio.setPayloadSize(sizeof(payload)); // float datatype occupies 4 bytes + + // Set the PA Level low to try preventing power supply related problems + // because these examples are likely run with nodes in close proximity to + // each other. + radio.setPALevel(RF24_PA_LOW); // RF24_PA_MAX is default. + + // set the TX address of the RX node into the TX pipe + radio.openWritingPipe(address[radioNumber]); // always uses pipe 0 + + // set the RX address of the TX node into a RX pipe + radio.openReadingPipe(1, address[!radioNumber]); // using pipe 1 + + // For debugging info + // radio.printDetails(); // (smaller) function that prints raw register values + // radio.printPrettyDetails(); // (larger) function that prints human readable data + + // ready to execute program now + if(argc!=3){ + printf("Usage : %s <IP> <port>\n",argv[0]); + exit(1); + } + clnt_sock = socket(PF_INET, SOCK_STREAM, 0); + if(clnt_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 = inet_addr(argv[1]); + serv_addr.sin_port = htons(atoi(argv[2])); + + if(connect(clnt_sock, (struct sockaddr*)&serv_addr, sizeof(serv_addr))==-1) + error_handling("connect() error"); + + if(-1 == GPIOExport(POUT2) || -1 == GPIOExport(PIN)) + { + return(1); + } + + + if(-1 == GPIODirection(POUT2, OUT) || -1 == GPIODirection(PIN, IN)) + { + return(2); + } + + PWMExport(PWM); + PWMWritePeriod(PWM, 20000000); + PWMWriteDutyCycle(PWM, 0); + PWMEnable(PWM); + + + do { + if (-1 == GPIOWrite(POUT2, 1)) { + return 3; + } + + str_len = read(clnt_sock, msg, sizeof(msg)); + if(str_len == -1) + error_handling("read() error"); + + printf("Receive message from Server : %s\n",msg); + /*if(strncmp(on,msg,1)==0) + light = 1; + else + light = 0; + + GPIOWrite(POUT, light);*/ + int pinValue = GPIORead(PIN); + printf("GPIORead: %d from pin %d\n", pinValue, PIN); + printf("angle: %d\n", angle); + + // 상태 변화 감지 + if (pinValue != prev_state) { + state = pinValue; + + } + // 문자열을 정수로 변환 + float a = atoi(msg); + + if(a == 1){ + // GPIORead(PIN)의 값이 0일 때에만 서보모터 제어 + if (state == 0) { + PWMWriteDutyCycle(PWM, angle); + printf("angle: %d\n", angle); + angle += 50000*direction; // 각도 변경 방향에 따라 각도 증감 + usleep(100000); + // 최대 각도 또는 최소 각도에 도달하면 방향 변경 + if (angle >= MAX_ANGLE || angle <= MIN_ANGLE) { + direction *= -1; + usleep(5000000); + //c=최대각도 혹은 최소각도에 도달했을 때 잠깐 멈추기 + } + } + }else{ + PWMWriteDutyCycle(PWM, angle); + printf("angle: %d\n", angle); + + if (angle > MIN_ANGLE) { + printf("angle: %d\n", angle); + angle -= 50000*direction; //서보모터 각도 감소(사람 없다고 판단시 리프트 자동 내려감) + usleep(100000); + } + + } + prev_state = pinValue; + + radio.stopListening(); // put radio in TX mode + + + + unsigned int failure = 0; // keep track of failures + payload = a; + while (failure < 2) { + clock_gettime(CLOCK_MONOTONIC_RAW, &startTimer); // start the timer + bool report = radio.write(&payload, sizeof(float)); // transmit & save the report + uint32_t timerElapsed = getMicros(); // end the timer + + if (report) { + // payload was delivered + cout << "Transmission successful! Time to transmit = "; + cout << timerElapsed; // print the timer result + cout << " us. Sent: " << payload << endl; // print payload sent + } + else { + // payload was not delivered + cout << "Transmission failed or timed out" << endl; + + } + failure++; + + // to make this example readable in the terminal + delay(1000); // slow transmissions down by 1 second + } + cout << failure << " failures detected. Leaving TX role." << endl; + + + + }while(repeat--); + + + + if( -1 == GPIOUnexport(POUT2) || -1 == GPIOUnexport(PIN)) + { + return(4); + } + + close(clnt_sock); + //Disable GPIO pins + /*if (-1 == GPIOUnexport(POUT)) + return(4);*/ + + + return 0; +} + + +/** + * Calculate the elapsed time in microseconds + */ +uint32_t getMicros() +{ + // this function assumes that the timer was started using + // `clock_gettime(CLOCK_MONOTONIC_RAW, &startTimer);` + + clock_gettime(CLOCK_MONOTONIC_RAW, &endTimer); + uint32_t seconds = endTimer.tv_sec - startTimer.tv_sec; + uint32_t useconds = (endTimer.tv_nsec - startTimer.tv_nsec) / 1000; + + return ((seconds)*1000 + useconds) + 0.5; +}