Projektarbeit Line Following Robot bei Prof. Chowanetz im WS22/23
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

uart_communication.cpp 5.1KB

1 year ago
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150
  1. #include "uart_communication.h"
  2. LFR_UART::LFR_UART() : fileDescriptor(-1) {
  3. this->last =std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
  4. this->openSerialPort();
  5. this->configureSerialPort();
  6. }
  7. LFR_UART::~LFR_UART() {
  8. this->closeSerialPort();
  9. }
  10. void LFR_UART::openFile(const char *fileName) {
  11. this->fileDescriptor = open(fileName, O_RDWR | O_NONBLOCK);
  12. }
  13. int LFR_UART::writeDataToFile(int8_t *buff, uint32_t bufferLength) {
  14. //std::cout << "Sending uart: " << std::bitset<8>(buff[0]) << std::endl;
  15. 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;
  16. return write(this->fileDescriptor, buff, bufferLength);
  17. }
  18. int LFR_UART::readFromFile(int8_t *buff, uint32_t bufferLength) {
  19. return read(this->fileDescriptor, buff, bufferLength);
  20. }
  21. int LFR_UART::closeFile() {
  22. return close(this->fileDescriptor);
  23. }
  24. int8_t LFR_UART::doubleToByte(double in){
  25. /*
  26. * Map the range of -1.0 to 1.0 as double to the range of a byte. (int8 range)
  27. */
  28. double minDouble = -1.0;
  29. double maxDouble = 1.0;
  30. double rangeDouble = maxDouble - minDouble;
  31. double minByte = -128.0;
  32. double maxByte = 127.0;
  33. double rangeByte = maxByte - minByte;
  34. double progress = in - minDouble;
  35. double progressPercent = progress/rangeDouble;
  36. double inputInByteRange = minByte + progressPercent*rangeByte;
  37. return int8_t(inputInByteRange);
  38. }
  39. double LFR_UART::byteToDouble(int8_t in){
  40. double minDouble = -1.0;
  41. double maxDouble = 1.0;
  42. double rangeDouble = maxDouble - minDouble;
  43. double minByte = -128.0;
  44. double maxByte = 127.0;
  45. double rangeByte = maxByte - minByte;
  46. double progress = double(in) - minByte;
  47. double progressPercent = progress/rangeByte;
  48. double inputInDoubleRange = minDouble + progressPercent*rangeDouble;
  49. return inputInDoubleRange;
  50. }
  51. void LFR_UART::sendTelegram(double wheel1, double wheel2, double wheel3, double wheel4){
  52. std::chrono::milliseconds now = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
  53. unsigned int deltaMs = static_cast<unsigned int>((now-last).count());
  54. if (deltaMs < 50 && (std::fabs(wheel1)+std::fabs(wheel2)+std::fabs(wheel3)+std::fabs(wheel4)) > 0.0005)
  55. {
  56. std::cout << "Too fast" << std::endl;
  57. return;
  58. }
  59. last = now;
  60. if(wheel1 > 1.0 || wheel2 > 1.0 || wheel3 > 1.0 || wheel4 > 1.0){
  61. throw CommunicatorException("Wheel value must not be greater than 1.0");
  62. }
  63. if(wheel1 < -1.0 || wheel2 < -1.0 || wheel3 < -1.0 || wheel4 < -1.0){
  64. throw CommunicatorException("Wheel value must not be smaller than -1.0");
  65. }
  66. // Discrepancy between the numbering of the wheels in App/Pi and the µc
  67. // Our - their
  68. // 1 - 4
  69. // 2 - 2
  70. // 3 - 3
  71. // 4 - 1
  72. int8_t wheel1B = this->doubleToByte(wheel4);
  73. //int8_t wheel1B = this->doubleToByte(wheel1);
  74. int8_t wheel2B = this->doubleToByte(wheel2);
  75. int8_t wheel3B = this->doubleToByte(wheel3);
  76. int8_t wheel4B = this->doubleToByte(wheel1);
  77. //int8_t wheel4B = this->doubleToByte(wheel4);
  78. int8_t checksum = wheel1B^wheel2B^wheel3B^wheel4B;
  79. int8_t telegram_buffer[5] = {wheel1B, wheel2B, wheel3B, wheel4B, checksum};
  80. uint32_t telegram_length = 5;
  81. this->writeDataToFile(telegram_buffer, telegram_length);
  82. }
  83. bool LFR_UART::readTelegram(double* buffer){
  84. int8_t tmp_buffer[5] = {0, 0, 0, 0, 0};
  85. uint32_t telegram_length = 5;
  86. this->readFromFile(tmp_buffer, telegram_length);
  87. //std::cout << "Read from file: " << std::bitset<8>(tmp_buffer[0]) << ", " << std::bitset<8>(tmp_buffer[1]) << ", " << std::bitset<8>(tmp_buffer[2]) << ", " << std::bitset<8>(tmp_buffer[3]) << ", " << std::bitset<8>(tmp_buffer[4]) << std::endl;
  88. if (std::bitset<8>(tmp_buffer[0]^tmp_buffer[1]^tmp_buffer[2]^tmp_buffer[3]) != std::bitset<8>(tmp_buffer[4])){
  89. return false;
  90. }
  91. else {
  92. buffer[0] = this->byteToDouble(tmp_buffer[0]);
  93. buffer[1] = this->byteToDouble(tmp_buffer[1]);
  94. buffer[2] = this->byteToDouble(tmp_buffer[2]);
  95. buffer[3] = this->byteToDouble(tmp_buffer[3]);
  96. }
  97. return true;
  98. }
  99. void LFR_UART::openSerialPort() {
  100. openFile(this->serialPortPath);
  101. if(this->fileDescriptor < 0) {
  102. throw CommunicatorException("LFR_UART couldn't open the serial port.");
  103. exit(EXIT_FAILURE);
  104. }
  105. }
  106. void LFR_UART::configureSerialPort() {
  107. if(tcgetattr(this->fileDescriptor, &this->tty)) {
  108. throw CommunicatorException("LFR_UART couldn't get the port attributes.");
  109. exit(EXIT_FAILURE);
  110. }
  111. cfsetispeed(&this->tty,B115200);
  112. cfsetospeed(&this->tty,B115200);
  113. cfmakeraw(&this->tty);
  114. if(tcsetattr(this->fileDescriptor,TCSANOW,&this->tty)) {
  115. throw CommunicatorException("LFR_UART couldn't set the port attributes.");
  116. exit(EXIT_FAILURE);
  117. }
  118. }
  119. void LFR_UART::closeSerialPort() {
  120. this->closeFile();
  121. }