Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include<stdio.h>
- #include<string.h>
- #include<string>
- #include<iostream>
- #include<thread>
- #include<fcntl.h>
- #include<errno.h>
- #include<termios.h>
- #include<unistd.h>
- #include<fstream>
- int main(){
- int serial_port = open("/dev/ttyACM0", O_RDWR);
- struct termios tty;
- memset(&tty, 0, sizeof(tty));
- if(tcgetattr(serial_port, &tty) != 0){
- printf("Error %i from tcgetattr: %s\n", errno, strerror(errno));
- }
- tty.c_cflag &= ~PARENB;
- tty.c_cflag &= ~CSTOP;
- tty.c_cflag |= CS8;
- tty.c_cflag &= ~CRTSCTS;
- tty.c_cflag |= CREAD | CLOCAL;
- tty.c_lflag &= ~ICANON;
- tty.c_lflag &= ~ECHO; // Disable echo
- tty.c_lflag &= ~ECHOE; // Disable erasure
- tty.c_lflag &= ~ECHONL; // Disable new-line echo
- tty.c_lflag &= ~ISIG; // Disable interpretation of INTR, QUIT and SUSP
- tty.c_iflag &= ~(IXON | IXOFF | IXANY); // Turn off s/w flow ctrl
- tty.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL); // Disable any special handling of received bytes
- tty.c_oflag &= ~OPOST; // Prevent special interpretation of output bytes (e.g. newline chars)
- tty.c_oflag &= ~ONLCR; // Prevent conversion of newline to carriage return/line feed
- tty.c_cc[VTIME] = 10; // Wait for up to 1s (10 deciseconds), returning as soon as any data is received.
- tty.c_cc[VMIN] = 0;
- // Set in/out baud rate to be 9600
- cfsetispeed(&tty, B9600);
- cfsetospeed(&tty, B9600);
- // Save tty settings, also checking for error
- if (tcsetattr(serial_port, TCSANOW, &tty) != 0) {
- printf("Error %i from tcsetattr: %s\n", errno, strerror(errno));
- }
- char read_buf [1];
- memset(&read_buf, '\0', sizeof(read_buf));
- // Strings to Hold the data
- std::string steeringAngle;
- std::string brakePosition;
- std::string gasPosition;
- std::string serialData;
- std::fstream myFile;
- //std::this_thread::sleep_for(std::chrono::milliseconds(5000)); // Wait time for cameras to start.
- bool readEnable = false;
- int iters = 0;
- while(true){
- //memset(&read_buf, '\0', sizeof(read_buf));
- // Read bytes. The behaviour of read() (e.g. does it block?,
- // how long does it block for?) depends on the configuration
- // settings above, specifically VMIN and VTIME
- int num_bytes = read(serial_port, &read_buf, sizeof(read_buf));
- // n is the number of bytes read. n may be 0 if no bytes were received, and can also be -1 to signal an error.
- if (num_bytes < 0) {
- printf("Error reading: %s", strerror(errno));
- }
- if (read_buf[0] == 'I') {
- serialData += read_buf[0];
- readEnable = true;
- }
- while (readEnable) {
- num_bytes = read(serial_port, &read_buf, sizeof(read_buf));
- if (num_bytes <= 0){
- continue;
- }
- if (read_buf[0] == '\0') {
- readEnable = false;
- break;
- }
- serialData += read_buf[0];
- }
- // Grabs correct can bus ID and save data
- if (serialData.find("0x156") != std::string::npos && steeringAngle.empty()) {
- std::cout << serialData << std::endl;
- steeringAngle = serialData;
- serialData.clear();
- }
- else if (serialData.find("0x17C") != std::string::npos && gasPosition.empty()) {
- std::cout << serialData << std::endl;
- gasPosition = serialData;
- serialData.clear();
- }
- else if (serialData.find("0x1A4") != std::string::npos && brakePosition.empty()) {
- std::cout << serialData << std::endl;
- brakePosition = serialData;
- serialData.clear();
- }
- // Checks if all three can bus ID strings are empty
- // Need all three filled before the program continues.
- //Saves to file
- if ((!steeringAngle.empty() && steeringAngle.length() > 1) &&
- (!gasPosition.empty() && gasPosition.length() > 1) &&
- (!brakePosition.empty() && brakePosition.length() > 1)) {
- iters++;
- myFile.open("carData.txt", std::ios_base::app);
- std::string tempData = steeringAngle + ";" +
- gasPosition + ";" +
- brakePosition + '\n';
- myFile << tempData;
- myFile.close();
- tempData.clear();
- steeringAngle.clear();
- gasPosition.clear();
- brakePosition.clear();
- // Tells threads to continue
- //enabled = true;
- //suspend_cv.notify_all();
- //std::this_thread::sleep_for(std::chrono::milliseconds(300));
- }
- if (iters == 10) {
- std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Lets all cams catch up if other cams are ahead.
- std::cout << "COMPLETE!!!" << std::endl;
- break;
- }
- }
- close(serial_port);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement