uint -> int; max send rate;

Communication/uart_communication.cpp

@@ -2,6 +2,7 @@
 
 
 LFR_UART::LFR_UART() : fileDescriptor(-1) {
+    this->last =std::chrono::duration_cast<std::chrono::milliseconds>(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::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
+    unsigned int deltaMs = static_cast<unsigned int>((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");
     }
-
-    uint8_t wheel1B = this->doubleToByte(wheel1);
-    uint8_t wheel2B = this->doubleToByte(wheel2);
-    uint8_t wheel3B = this->doubleToByte(wheel3);
-    uint8_t wheel4B = this->doubleToByte(wheel4);
-
-    uint8_t checksum = wheel1B^wheel2B^wheel3B^wheel4B;
-
-    uint8_t telegram_buffer[5] = {wheel1B, wheel2B, wheel3B, wheel4B, checksum}; 
+    // 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);
+
+    int8_t checksum = wheel1B^wheel2B^wheel3B^wheel4B;
+
+    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);
 

Communication/uart_communication.h

@@ -11,12 +11,15 @@
 #include <iostream>
 #include <exception>
 #include <iostream>
+#include <chrono>
 
 #include <bitset>
 
 
 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);