Communication Development

MATE_2019/Arduino/mainBoard/mainBoard.ino

@@ -44,7 +44,7 @@ String disabledCommand = ":1500;1500;1500;1500;1500;1500;1500;1500;1500;1500;0";
 void setup()
 {
 
-  delay(6000); 
+  delay(1000); 
 
   Wire.begin();
   SerialConnection.begin(115200);
@@ -86,7 +86,7 @@ void loop()
   static bool wireInit = false;
   while (!wireInit)
   {
-    Wire.beginTransmission(14);
+    Wire.beginTransmission(10);
     Wire.write("1500");
     Wire.write("1500");
     Wire.write(':');
@@ -112,7 +112,7 @@ void loop()
     delay(90);
     ++timer;
 
-    if (timer > 50)
+    if (timer > 5)
     {
       wireInit = true;
     }

MATE_2019/Main/Headers/SerialPort.h

@@ -1,9 +1,6 @@
-/* Zain Ul Mustafa 2017 */
-
 #ifndef SERIALPORT_H
 #define SERIALPORT_H
 
-#define ARDUINO_WAIT_TIME 2000
 #define MAX_DATA_LENGTH 255
 
 #include <stdio.h>
@@ -19,9 +16,11 @@ class SerialPort
   DWORD errors;
 
  public:
-  SerialPort(const char *portName, const int baudRate);
+  SerialPort();
   ~SerialPort();
 
+  void openSerialPort(const char *portName, const int baudRate);
+  void closeSerialPort();
   int readSerialPort(char *buffer, unsigned int buf_size);
   bool writeSerialPort(char *buffer, unsigned int buf_size);
   bool isConnected();

MATE_2019/Main/Sources/SerialPort.cpp

@@ -1,25 +1,21 @@
-#include "..\Headers\SerialPort.h"
+#include "../Headers/SerialPort.h"
 
-SerialPort::SerialPort(const char *portName, const int baudRate)
+SerialPort::SerialPort()
+{
+  this->connected = false;
+  this->handler = INVALID_HANDLE_VALUE;
+}
+
+SerialPort::~SerialPort() { closeSerialPort(); }
+
+void SerialPort::openSerialPort(const char *portName, const int baudRate)
 {
   this->connected = false;
 
   this->handler =
       CreateFileA(static_cast<LPCSTR>(portName), GENERIC_READ | GENERIC_WRITE,
                   0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL);
-  if (this->handler == INVALID_HANDLE_VALUE)
-  {
-    if (GetLastError() == ERROR_FILE_NOT_FOUND)
-    {
-      printf("ERROR: Handle was not attached. Reason: %s not available\n",
-             portName);
-    }
-    else
-    {
-      printf("ERROR!!!");
-    }
-  }
-  else
+  if (this->handler != INVALID_HANDLE_VALUE)
   {
     DCB dcbSerialParameters = {0};
 
@@ -43,19 +39,15 @@ SerialPort::SerialPort(const char *portName, const int baudRate)
       {
         this->connected = true;
         PurgeComm(this->handler, PURGE_RXCLEAR | PURGE_TXCLEAR);
-        Sleep(ARDUINO_WAIT_TIME);
       }
     }
   }
 }
 
-SerialPort::~SerialPort()
+void SerialPort::closeSerialPort() 
 {
-  if (this->connected)
-  {
-    this->connected = false;
-    CloseHandle(this->handler);
-  }
+  this->connected = false;
+  CloseHandle(this->handler);
 }
 
 int SerialPort::readSerialPort(char *buffer, unsigned int buf_size)

MATE_2019/Main/main.cpp

@@ -1,12 +1,14 @@
 #include <iostream>
 
-#include ".\Headers\Gamepad.h"
-#include ".\Headers\PID.h"
-#include ".\Headers\SerialPort.h"
-#include ".\Headers\Utils.h"
+#include "Headers/Gamepad.h"
+#include "Headers/PID.h"
+#include "Headers/SerialPort.h"
+#include "Headers/Utils.h"
 
 using namespace std;
 
+std::string port;
+
 bool disabled = false;
 bool waterLeak = false;
 
@@ -21,10 +23,7 @@ double rollSetpoint = 0.0;
 
 int yawOffset = 0;
 
-// Change the name of the port with the port name of your computer
-// Must remember that the backslashes are essential so do not remove them
-const char* port = "\\\\.\\COM3";
-SerialPort arduino(port, 115200);
+SerialPort arduino;
 Gamepad gamepad1 = Gamepad(1);
 Gamepad gamepad2 = Gamepad(2);
 
@@ -37,16 +36,28 @@ void transferData(string data)
   delete[] charArray;
 
   // Wait for arduino to process command
-  Sleep(90);
+  Sleep(75);
 
   // Expects 0 or 1, if water has been detected
   arduino.readSerialPort(output, MAX_DATA_LENGTH);
 
   cout << " Received: ";
 
+  bool checked = false;
+  int i = 0;
   for (char c : output)
   {
     cout << c;
+    if (!checked)
+    {
+      if (c == '\0')
+      {
+        arduino.closeSerialPort();
+        arduino.openSerialPort(port.c_str(), 115200);
+      }
+      checked = true;
+    }
+    output[i++] = '\0';
   }
   cout << endl << endl;
 
@@ -75,19 +86,19 @@ void transferData(string data)
 // rx ly lx
 void teleop(double FWD, double STR, double RCW)
 {
-  //cout << FWD << "\t" << STR << "\t" << RCW << endl;
+  // cout << FWD << "\t" << STR << "\t" << RCW << endl;
   // : is verification character for arduino
   string data = ":";
 
-  //PID pitchPID(0.007, 0.0, 0.0);
-  //pitchPID.setContinuous(false);
-  //pitchPID.setOutputLimits(-1.0, 1.0);
-  //pitchPID.setSetpoint(pitchSetpoint);
+  // PID pitchPID(0.007, 0.0, 0.0);
+  // pitchPID.setContinuous(false);
+  // pitchPID.setOutputLimits(-1.0, 1.0);
+  // pitchPID.setSetpoint(pitchSetpoint);
 
-  //PID rollPID(0.007, 0.0, 0.0);
-  //rollPID.setContinuous(false);
-  //rollPID.setOutputLimits(-1.0, 1.0);
-  //rollPID.setSetpoint(rollSetpoint);
+  // PID rollPID(0.007, 0.0, 0.0);
+  // rollPID.setContinuous(false);
+  // rollPID.setOutputLimits(-1.0, 1.0);
+  // rollPID.setSetpoint(rollSetpoint);
 
   // Let driver adjust angle of robot if necessary
   if (gamepad1.getButtonPressed(xButtons.A))
@@ -118,10 +129,10 @@ void teleop(double FWD, double STR, double RCW)
   // FR AND FL ARE SWAPPED
   // BR IS INVERTED?
   // WORKING VALUES 2/13/2020 W/ EXTENSION CORDS
-  double FR = (-STR * sin(heading) - FWD * sin(heading) + RCW);   // A
-  double BR = (STR * cos(heading) + FWD * sin(heading) - RCW);    // B
+  double FR = (-STR * sin(heading) - FWD * sin(heading) + RCW);  // A
+  double BR = (STR * cos(heading) + FWD * sin(heading) - RCW);   // B
   double BL = (-STR * sin(heading) + FWD * cos(heading) - RCW);  // C
-  double FL = (-STR * cos(heading) + FWD * cos(heading) - RCW);    // D
+  double FL = (-STR * cos(heading) + FWD * cos(heading) - RCW);  // D
 
   double UL = -(gamepad1.rightTrigger() - gamepad1.leftTrigger());
   double UR = -(gamepad1.rightTrigger() - gamepad1.leftTrigger());
@@ -200,12 +211,19 @@ void teleop(double FWD, double STR, double RCW)
 
 int main()
 {
+  std::cout << "Attempting to connect" << std::endl;
+  int i = 0;
   while (!arduino.isConnected())
   {
-    cout << " Error in Arduino port name" << endl << endl;
-  }
+    port = R"(\\.\COM)" + std::to_string(i++);
 
-  cout << " Arduino connection made" << endl << endl;
+    arduino.openSerialPort(port.c_str(), 115200);
+    if (i > 15)
+    {
+      i = 0;
+    }
+  }
+  std::cout << "Connected" << std::endl;
 
   if (gamepad1.connected())
   {