diff --git a/Communication/uart_communication.cpp b/Communication/uart_communication.cpp index c7c948a..46edb81 100644 --- a/Communication/uart_communication.cpp +++ b/Communication/uart_communication.cpp @@ -2,6 +2,7 @@ LFR_UART::LFR_UART() : fileDescriptor(-1) { + this->last =std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()); this->openSerialPort(); this->configureSerialPort(); } @@ -13,12 +14,13 @@ void LFR_UART::openFile(const char *fileName) { this->fileDescriptor = open(fileName, O_RDWR | O_NONBLOCK); } -int LFR_UART::writeDataToFile(uint8_t *buff, uint32_t bufferLength) { - //std::cout << "Sending uart telegram" << std::endl; +int LFR_UART::writeDataToFile(int8_t *buff, uint32_t bufferLength) { + //std::cout << "Sending uart: " << std::bitset<8>(buff[0]) << std::endl; + std::cout << "Sending uart: " << std::bitset<8>(buff[0]) << ", " << std::bitset<8>(buff[1]) << ", " << std::bitset<8>(buff[2]) << ", " << std::bitset<8>(buff[3]) << " csum: " << std::bitset<8>(buff[4]) << std::endl; return write(this->fileDescriptor, buff, bufferLength); } -int LFR_UART::readFromFile(uint8_t *buff, uint32_t bufferLength) { +int LFR_UART::readFromFile(int8_t *buff, uint32_t bufferLength) { return read(this->fileDescriptor, buff, bufferLength); } @@ -26,37 +28,32 @@ int LFR_UART::closeFile() { return close(this->fileDescriptor); } -uint8_t LFR_UART::doubleToByte(double in){ +int8_t LFR_UART::doubleToByte(double in){ /* - * Map the range of -1.0 to 1.0 as double to the range of a byte. - * - * -1 -> 0 - * 0.0 -> 127 - * 1.0 -> 254 - * Not using the full range upto 254 to hit the 0.0 + * Map the range of -1.0 to 1.0 as double to the range of a byte. (int8 range) */ double minDouble = -1.0; double maxDouble = 1.0; double rangeDouble = maxDouble - minDouble; - double minByte = 0.0; - double maxByte = 255.0; + double minByte = -128.0; + double maxByte = 127.0; double rangeByte = maxByte - minByte; double progress = in - minDouble; double progressPercent = progress/rangeDouble; double inputInByteRange = minByte + progressPercent*rangeByte; - return uint8_t(inputInByteRange); + return int8_t(inputInByteRange); } -double LFR_UART::byteToDouble(uint8_t in){ +double LFR_UART::byteToDouble(int8_t in){ double minDouble = -1.0; double maxDouble = 1.0; double rangeDouble = maxDouble - minDouble; - double minByte = 0.0; - double maxByte = 255.0; + double minByte = -128.0; + double maxByte = 127.0; double rangeByte = maxByte - minByte; double progress = double(in) - minByte; @@ -67,27 +64,43 @@ double LFR_UART::byteToDouble(uint8_t in){ } void LFR_UART::sendTelegram(double wheel1, double wheel2, double wheel3, double wheel4){ + std::chrono::milliseconds now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()); + unsigned int deltaMs = static_cast((now-last).count()); + if (deltaMs < 250) + { + std::cout << "Too fast" << std::endl; + return; + } + last = now; + if(wheel1 > 1.0 || wheel2 > 1.0 || wheel3 > 1.0 || wheel4 > 1.0){ throw CommunicatorException("Wheel value must not be greater than 1.0"); } if(wheel1 < -1.0 || wheel2 < -1.0 || wheel3 < -1.0 || wheel4 < -1.0){ throw CommunicatorException("Wheel value must not be smaller than -1.0"); } + // Discrepancy between the numbering of the wheels in App/Pi and the µc + // Our - their + // 1 - 4 + // 2 - 2 + // 3 - 3 + // 4 - 1 + int8_t wheel1B = this->doubleToByte(wheel4); + //int8_t wheel1B = this->doubleToByte(wheel1); + int8_t wheel2B = this->doubleToByte(wheel2); + int8_t wheel3B = this->doubleToByte(wheel3); + int8_t wheel4B = this->doubleToByte(wheel1); + //int8_t wheel4B = this->doubleToByte(wheel4); - uint8_t wheel1B = this->doubleToByte(wheel1); - uint8_t wheel2B = this->doubleToByte(wheel2); - uint8_t wheel3B = this->doubleToByte(wheel3); - uint8_t wheel4B = this->doubleToByte(wheel4); + int8_t checksum = wheel1B^wheel2B^wheel3B^wheel4B; - uint8_t checksum = wheel1B^wheel2B^wheel3B^wheel4B; - - uint8_t telegram_buffer[5] = {wheel1B, wheel2B, wheel3B, wheel4B, checksum}; + int8_t telegram_buffer[5] = {wheel1B, wheel2B, wheel3B, wheel4B, checksum}; uint32_t telegram_length = 5; this->writeDataToFile(telegram_buffer, telegram_length); } bool LFR_UART::readTelegram(double* buffer){ - uint8_t tmp_buffer[5] = {0, 0, 0, 0, 0}; + int8_t tmp_buffer[5] = {0, 0, 0, 0, 0}; uint32_t telegram_length = 5; this->readFromFile(tmp_buffer, telegram_length); diff --git a/Communication/uart_communication.h b/Communication/uart_communication.h index b8e1b3a..6c84a7f 100644 --- a/Communication/uart_communication.h +++ b/Communication/uart_communication.h @@ -11,12 +11,15 @@ #include #include #include +#include #include class LFR_UART { +public: + std::chrono::milliseconds last; int fileDescriptor; const char* serialPortPath = "/dev/ttyS0"; @@ -29,13 +32,14 @@ class LFR_UART void configureSerialPort(); void closeSerialPort(); - int writeDataToFile(uint8_t *buff, uint32_t bufferLength); - int readFromFile(uint8_t *buff, uint32_t bufferLength); + + int writeDataToFile(int8_t *buff, uint32_t bufferLength); + int readFromFile(int8_t *buff, uint32_t bufferLength); public: - uint8_t doubleToByte(double in); - double byteToDouble(uint8_t in); + int8_t doubleToByte(double in); + double byteToDouble(int8_t in); void sendTelegram(double wheel1, double wheel2, double wheel3, double wheel4); bool readTelegram(double* buffer);