Too big too handle

AutonomousMode/ControlModule/control_module.cpp

@@ -36,7 +36,7 @@ void ControlModule::adjustSpeed(){
 
     for(int i = 0; i < 4; i++)
     {
-        motors[i] *= maxSpeed;
+        motors[i] *= factor;
     }
 };
 
@@ -85,7 +85,7 @@ void ControlModule::calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, d
     std::unique_lock<std::mutex> lock(mtx);
     double rotationRatio = ratio(angle);
     rotate(rotationRatio);
-    drive(1.0-rotationRatio);
+    drive(1.0-std::fabs(rotationRatio));
     unit();
     adjustSpeed();
 }
@@ -102,13 +102,21 @@ double ControlModule::ratio(double angle)
     double maxAngularSpeed = 1.0;
     double speedRange = maxAngularSpeed - minAngularSpeed;
 
-    double minAngle = -90.0;
-    double maxAngle = 90.0;
+    double minAngle = -20.0;
+    double maxAngle = 20.0;
     double angularRange = maxAngle - minAngle;
 
+    if(angle < minAngle)
+    {
+        angle = minAngle;
+    }
+    if(angle > maxAngle)
+    {
+        angle = maxAngle;
+    }
     double progress = angle - minAngle;
     double progressPercent = progress/angularRange;
 
     double speed = minAngularSpeed + progressPercent*speedRange;
-    return abs(speed);
+    return speed;
 }

AutonomousMode/ControlModule/control_module.h

@@ -2,6 +2,7 @@
 #include <mutex>
 #include <vector>
 #include <iostream>
+#include <cmath>
 
 class ControlModule
 {

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 < 50 && (std::fabs(wheel1)+std::fabs(wheel2)+std::fabs(wheel3)+std::fabs(wheel4)) > 0.0005)
+    {
+        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);
 

Communication/uart_communication.h

@@ -11,12 +11,16 @@
 #include <iostream>
 #include <exception>
 #include <iostream>
+#include <chrono>
+#include <cmath>
 
 #include <bitset>
 
 
 class LFR_UART
 {
+public:
+	std::chrono::milliseconds last;
 	int fileDescriptor;
 	const char* serialPortPath = "/dev/ttyS0";
 	
@@ -29,13 +33,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);

MobileApp/app/src/main/java/com/example/lfrmobileapp/Communication.java

@@ -29,12 +29,12 @@ public class Communication {
     }
 
     public String telegram(boolean startStop, ArrayList<String> list){
-        String tel = "";
+        String tel = "aa";
         tel += "1"; //Autonomous mode
         if(startStop){
             tel += ";1";
         }else{
-            tel += ";0";
+            return "aa0;0.0;0.0;0.0;0.0zz";
         }
 
         for (String i : list) {
@@ -44,6 +44,7 @@ public class Communication {
                 tel += ";1";
             }
         }
+        tel += "zz";
         return tel;
     }
 

MobileApp/app/src/main/java/com/example/lfrmobileapp/ui/home/HomeFragment.java

@@ -75,18 +75,13 @@ public class HomeFragment extends Fragment {
             @SuppressLint("ClickableViewAccessibility")
             @Override
                 public boolean onTouch(View v, MotionEvent event) {
-                boolean send = false;
-                if (System.currentTimeMillis() - lastOnMoveCall > 250){
-                    send = true;
-                    lastOnMoveCall = System.currentTimeMillis();
-                }
-                if (send && mainActivity.connected) {
+                if (mainActivity.connected) {
                     if (event.getAction() == MotionEvent.ACTION_UP) {
                         binding.rotateLeft.performClick();
                         mainActivity.sendTelegram(com.telegram(new int[]{0, 0, 0, 0}, 0));
                         return true;
                     } else {
-                        mainActivity.sendTelegram(com.telegram(new int[]{-1, 1, -1, 1}, 100));
+                        mainActivity.sendTelegram(com.telegram(new int[]{-1, 1, -1, 1}, 50));
                     }
                 }
                 return false;
@@ -97,18 +92,13 @@ public class HomeFragment extends Fragment {
             @SuppressLint("ClickableViewAccessibility")
             @Override
             public boolean onTouch(View v, MotionEvent event) {
-                boolean send = false;
-                if (System.currentTimeMillis() - lastOnMoveCall > 250){
-                    send = true;
-                    lastOnMoveCall = System.currentTimeMillis();
-                }
-                if (send && mainActivity.connected){
+                if (mainActivity.connected){
                     if(event.getAction() == MotionEvent.ACTION_UP){
                         binding.rotateRight.performClick();
                         mainActivity.sendTelegram(com.telegram(new int[]{0, 0, 0, 0}, 0));
                         return true;
                     }else{
-                        mainActivity.sendTelegram(com.telegram(new int[]{1, -1, 1, -1}, 100));
+                        mainActivity.sendTelegram(com.telegram(new int[]{1, -1, 1, -1}, 50));
                     }
                 }
                 return false;
@@ -157,6 +147,7 @@ public class HomeFragment extends Fragment {
                 else{
                     joystick.setBackgroundResource(R.mipmap.blank);
                     telegram = com.telegram(new int[]{0, 0, 0, 0}, strength);
+                    send=true;
                 }
                 if (send && mainActivity.connected)
                 {

Socket/lfr_socket.cpp

@@ -2,6 +2,7 @@
 
 LFR_Socket::LFR_Socket(ExceptionCallback cb): cb(cb)
 {
+    bufferIterator = std::begin(buffer);
     if ((server_fd = socket(AF_INET, SOCK_STREAM, 0)) == -1)
     {
         throw std::runtime_error("Failed opening the socket");
@@ -66,7 +67,7 @@ void LFR_Socket::createThread()
         bool connected = false;
         while(true)
         {
-            LFR_Socket::LFR_Telegram telegram;
+            LFR_Socket::LFR_Telegram telegram = {};
             bool received = false;
             if(!stop && !listeners.empty())
             {
@@ -91,32 +92,46 @@ void LFR_Socket::createThread()
                                 }
                                 else
                                 {
-                                    //std::cout << "accepted connection" << std::endl;
+                                    std::cout << "accepted connection" << std::endl;
                                     pollfds[1].fd = new_socket;
                                     connected  = true;
                                 }
                             }
                             else if (connected  && pollfds[0].revents & POLLIN)
                             {
-                                //std::cout << "second connection incoming" << std::endl;
+                                std::cout << "second connection incoming" << std::endl;
                                 int tmp_socket = accept(server_fd, (struct sockaddr *)&address, (socklen_t *)&addrlen);
                                 close(tmp_socket);
-                                //std::cout << "second connection closed" << std::endl;
+                                std::cout << "second connection closed" << std::endl;
                             }
                             else if (pollfds[1].revents & POLLIN)
                             {
-                                bytes_received = recv(new_socket, buffer, sizeof(buffer), 0);
-                                if (bytes_received > 0)
+                                LFR_Telegram tmpBuffer = {0};
+                                bytesReceived = recv(new_socket, tmpBuffer, sizeof(tmpBuffer), 0);
+                                if (bytesReceived > 0)
                                 {
-                                    received = true;
-                                    std::copy(std::begin(buffer), std::end(buffer), std::begin(telegram));
-                                    //std::cout << "message received" << std::endl;
+                                    if(bufferIterator - std::begin(buffer) + bytesReceived > 1024)
+                                    {
+                                        //Too long for buffer. Clear it.
+                                        std::fill(std::begin(buffer), std::end(buffer), 0);
+                                        bufferIterator = std::begin(buffer);
+                                    }
+                                    bufferIterator = std::copy_if(
+                                        std::begin(tmpBuffer),
+                                        std::begin(tmpBuffer)+bytesReceived, 
+                                        bufferIterator,
+                                        [](char c){
+                                            return(c != '\n' && c != '\r');
+                                        });
+
+                                    //std::cout << "bytes received: " << bytesReceived << std::endl;
+                                    //received = true;
                                 }
-                                else if(bytes_received == 0)
+                                else if(bytesReceived == 0)
                                 {
                                     connected = false;
                                     close(pollfds[1].fd);
-                                    //std::cout << "connection closed by peer" << std::endl;
+                                    std::cout << "connection closed by peer" << std::endl;
                                 }
                             }
                         }
@@ -131,6 +146,32 @@ void LFR_Socket::createThread()
                     }
                 }
 
+                // Search the buffer for the start of a telegram
+                char *telegramStart = NULL;
+                telegramStart = strstr(buffer, "aa");
+                if(telegramStart && telegramStart != std::begin(buffer)) {
+                    //Move the content of the buffer
+                    auto delta = bufferIterator - telegramStart;
+                    std::copy(telegramStart, std::end(buffer), std::begin(buffer));
+                    bufferIterator = std::begin(buffer) + delta;
+                }
+
+                // Search the buffer for the end of a telegram
+                char *telegramEnd = NULL;
+                telegramEnd = strstr(buffer, "zz");
+                if(telegramEnd && telegramStart && telegramEnd-telegramStart>2) {
+                    std::copy(telegramStart+2, telegramEnd, telegram);
+                    received = true;
+                    std::copy(telegramEnd+2, std::end(buffer), std::begin(buffer));
+                    bufferIterator = std::begin(buffer);
+                }
+                else if(telegramEnd && telegramStart && telegramEnd-telegramStart<=2)
+                {
+                    //Got an empty telegram
+                    std::copy(telegramEnd, std::end(buffer), std::begin(buffer));
+                    bufferIterator = std::begin(buffer);
+                }
+                
                 //Invoke the callback method (ListenerPair second -> ListenerCallback)
                 if (received)
                 {

Socket/lfr_socket.h

@@ -13,6 +13,7 @@
 #include <string>
 #include <algorithm>
 #include <exception>
+#include <cstring>
 
 class LFR_Socket
 {
@@ -31,11 +32,12 @@ private:
     std::unique_ptr<std::thread> thread;
     mutable std::mutex mutex;
 
-    int server_fd, new_socket, bytes_received;
+    int server_fd, new_socket, bytesReceived;
     struct sockaddr_in address;
     int opt = 1;
     int addrlen = sizeof(address);
     char buffer[1024] = {0};    
+    char* bufferIterator;
 
     
     //void provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi);

lfr_state_machine.cpp

@@ -57,15 +57,19 @@ void LFR_StateMachine::parseString(string s)
     std::vector<std::string> splitStr = split(s, ";");
     if(!checkStringValidity(splitStr))
     {
-        std::cout<<"Invalid String" << std::endl;
+        std::cout<<"Error: Invalid String: " << s << std::endl;
         return;
     }
+    else
+    {
+        std::cout<< s << std::endl;
+    }
     double wheels[4] = {0.0, 0.0, 0.0, 0.0};
     int mode = std::stoi(splitStr[0]);
     if(mode == 0) {
-        for(int i = 1; i < 4; i++)
+        for(int i = 1; i <= 4; i++)
         {
-            wheels[i] = std::stod(splitStr[i]);
+            wheels[i-1] = std::stod(splitStr[i]);
         }
         setState(State::Manual::getInstance());
         uartCommunicator.sendTelegram(wheels[0], wheels[1], wheels[2], wheels[3]);
@@ -100,16 +104,26 @@ LFR_StateMachine::LFR_StateMachine():
     currentState = &State::Idle::getInstance();
     currentState->enter(this);
 
-    //Start the permanent loop
-    char input;
-    std::cout << "press q to quit" << std::endl;
-    std::cin >> input;
-    std::cout << "binned" << std::endl;
-    while (input != 'q')
+    cv::VideoWriter writer = cv::VideoWriter("video_200.avi", cv::VideoWriter::fourcc('M','J','P','G'), 4.0, cv::Size(videoWidth, videoHeight), true);
+    
+    auto t_start = std::chrono::high_resolution_clock::now();
+    auto t_end = std::chrono::high_resolution_clock::now();
+    double dur = std::chrono::duration<double, std::milli>(t_end-t_start).count();
+    
+    while(dur < 60000)
     {
-        std::cin >> input;
-        std::cout << "binned" << std::endl;
+        t_end = std::chrono::high_resolution_clock::now();
+        dur = std::chrono::duration<double, std::milli>(t_end-t_start).count();
+        {
+            this_thread::sleep_for(std::chrono::milliseconds(50));
+            std::unique_lock<std::mutex> lock(imgMutex);
+            if(!this->image.empty())
+            {
+                writer.write(this->image);
+            }
+        }
     }
+    writer.release();
     std::cout << "Exiting central" << std::endl;
 }
 
@@ -121,11 +135,6 @@ void LFR_StateMachine::setState(LFR_IState& newState)
         currentState = &newState;
         currentState->enter(this);
     }
-    else
-    {
-        std::cout << "no switch" << std::endl;
-    }
-
 }
 
 void LFR_StateMachine::enterAutonomous()
@@ -147,6 +156,10 @@ void LFR_StateMachine::enterAutonomous()
             double delta = static_cast<double>(deltaMs) / 1000.0;
             double frameRate = 1.0 / static_cast<double>(delta);*/
             double frameRate = -1.0;
+            {
+                std::unique_lock<std::mutex> lock(imgMutex);
+                image = result.rawImage;
+            }
 
             if (result.validLane)
             {

lfr_state_machine.h

@@ -16,7 +16,7 @@ class LFR_StateMachine
     const int videoHeight = 720;
     const int videoWidth = 1280;
     const int gaussKernelSize = 11;
-    const double maxSpeed = 0.5;
+    const double maxSpeed = 0.20;
 
     std::mutex mutex;
 
@@ -24,6 +24,9 @@ class LFR_StateMachine
     LFR_UART uartCommunicator;
     LFR_Socket socket;
 
+    std::mutex imgMutex;
+    Mat image;
+
     vector<string> split (string s, string delimiter) const;
     void sanitize (string& s) const;
     bool checkStringValidity (const std::vector<std::string>& s) const;
@@ -36,3 +39,5 @@ public:
     inline LFR_IState* getCurrentState() const {return currentState;}
     void setState(LFR_IState& newState);
 };
+
+