Speed up Serial Communication to Arduino c++ - c++

Question

First time posting so I apologize.
I am trying to make a ball and beam project using a camera as the sensor and trying to get the serial communication to work.
The code currently works if I set the waitKey to (1000) but that is not fast enough for the project to work and was wanting some help speeding it up.
the output tends to only read the first string that I send then ignores anything else until the program is paused by holding the slider on the output screen for a couple of seconds.
I have tried to use a faster baud rate but the problem still persists.
This is the C++ code
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <iostream>
#include <vector>
#include "SerialPort.h"
#include <string.h>
#include <stdlib.h>
#include <string>
using namespace cv;
using namespace std;
//global for the image processing
Mat imgGray, imgCanny, imgBlur, imgDil;
Mat imgHSV, mask;
//int hmin = 53, smin = 102, vmin = 35; //green
//int hmax = 93, smax = 253, vmax = 169;
/* //green ball
int hmin = 41, smin = 93, vmin = 49;
int hmax = 93, smax = 243, vmax = 192; //p
*/
//purple ball
int hmin = 117, smin = 26, vmin = 63;
int hmax = 179, smax = 113, vmax = 169;
//global for the serial
char output[MAX_DATA_LENGTH];
char incomingData[MAX_DATA_LENGTH];
char port[] = " ////.//COM3";
int PosX;
// this will convert the output into the correct string because the "stoi" function doesn't work as intended
String intToString(int I)
{
String J;
if (I ==80 )
{
J ="80" ;
}
else if (I ==81 )
{
J = "81";
}
else if (I ==82 )
{
J = "82";
}
else if (I == 83)
{
J = "83";
}
else if (I ==84 )
{
J = "84";
}
else if (I == 85)
{
J = "85";
}
else if (I == 86)
{
J = "86";
}
else if (I ==87 )
{
J = "87";
}
else if (I ==88 )
{
J = "88";
}
else if (I ==89 )
{
J = "89";
}
else if (I ==90 )
{
J = "90";
}
else if (I ==91 )
{
J = "91";
}
else if (I == 92)
{
J = "92";
}
else if (I == 93)
{
J = "93";
}
else if (I == 94)
{
J = "94";
}
else if (I == 95)
{
J = "95";
}
else if (I == 96)
{
J = "96";
}
else if (I == 97)
{
J = "97";
}
else if (I == 98)
{
J = "98";
}
else if (I == 99)
{
J = "99";
}
else if (I == 100)
{
J = "100";
}
else if (I > 100)
{
J = "100";
}
else if (I < 80)
{
J = "80";
}
return (J);
}
// calculation to make the output between outMin and outMax taken from arduino website and transformed into calculation
int map(int x,int in_min, int in_max, int out_min, int out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
// just the P of the PID controller
int PCONTROL(int xPos)
{
int P, Kp = 1, Proportional, error;
error = 240 - xPos; //240 is the middle of the camera and xPos is the current position
Proportional = Kp * error;
P = map(Proportional,-240,240,80,100); // calculation to make the output between 80&100 taken from arduino website and transformed into calculation
return (P);
}
// this is the function made to be the PID contol loop
int PIDCONTROL(int xPos)
{
int error_prior = 0, error, integral_prior = 0, Kp, Ki,
Kd, integral, derivative, proportional, outPut, PID_total, period = 50;
// I have set the Kp and Ki to zero when testing the P contoller
Kp = 1;
Ki = 0;
Kd = 0;
//error = (img.rows/2) - xPos; // 640 is the desired Value
error = (480/2) - xPos; // this is if I got the wrong part of the resolution
proportional = Kp * error;
integral = integral_prior + error;
derivative = (error - error_prior)/period;
PID_total = ((Kp * proportional) + (Ki * integral) + (Kd * derivative)); //this is the calculation for the PID controller
error_prior = error;
integral_prior = integral;
outPut = map(PID_total,-240,240,80,100);
cout << "output = " << outPut << endl;
return (outPut);
}
// this code will get the x and y values of the ball, draw a box around it and lable it
// this is what i need for the project
void getContours(Mat imgDil, Mat img)
{
vector<vector<Point>> contours;
vector<Vec4i> hierarchy;
string objectType;
string x, y;
findContours(imgDil, contours, hierarchy, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
//drawContours(img, contours, -1, Scalar(255, 0, 255), 2); //draws everything
for (int i = 0; i < contours.size(); i++)
{
int area = contourArea(contours[i]);
//cout << area << endl; //displays the area of the ball
vector<vector<Point>>conPoly(contours.size());
vector<Rect>boundRect(contours.size());
if (area > 1000 && area < 5000) //filters out smaller and larger objects
{
float peri = arcLength(contours[i], true);
approxPolyDP(contours[i], conPoly[i], 0.02 * peri, true);
drawContours(img, contours, i, Scalar(255, 0, 255), 2); // with a filter applyed
boundRect[i] = boundingRect(conPoly[i]); //draw a box around the ball
rectangle(img, boundRect[i].tl(), boundRect[i].br(), Scalar(0, 255, 0), 5);
putText(img, "Ball", { boundRect[i].x,boundRect[i].y - 5 }, FONT_HERSHEY_PLAIN, 1, Scalar(0, 69, 255), 2); //display text above the box saying ball
//cout << "X = " << boundRect[i].x << endl;// << "Y = " << boundRect[i].y << endl; //print the x and y values
PosX = boundRect[i].x;
}
}
}
void main()
{
VideoCapture cap(0);
Mat img;
string xposSrg;
//int TempData;
Mat Reimg, Reimg2;
string data;
//char TrackbarName[50];
const int alpha_slider_max = 100;
int xPos, preData = 0;
createTrackbar("Hue Min", "Trackbars", &hmin, 179);
SerialPort arduino(port);
if (arduino.isConnected()) {
cout << "Connection" << endl << endl;
}
else {
cout << "Error Connecting" << endl << endl;
}
while (arduino.isConnected()) // no point do it if not connected
{
cap.read(img);
//prepocessing
Mat imgGray, imgCanny, imgBlur,Reimg,Reimg2;
//cvtColor(img, imgGray, COLOR_BGR2GRAY);
//pyrDown(img, Reimg, Size(img.cols / 2, img.rows / 2)); // downsample for faster processing
cvtColor(img, imgHSV, COLOR_BGR2HSV); // get the big wierd colours
Scalar lower(hmin, smin, vmin);
Scalar upper(hmax, smax, vmax);
inRange(imgHSV, lower, upper, mask); // make only the green ball visible
GaussianBlur(mask, imgBlur, Size(3, 3), 3, 0);
Canny(imgBlur, imgCanny, 25, 75);
Mat kernel = getStructuringElement(MORPH_RECT, Size(3, 3));
dilate(imgCanny, imgDil, kernel);
getContours(imgDil, img); // use only the visible green to the contours fuction
//displaying processes used in debuging
/*
imshow("Ball", img);
imshow("Gray", imgGray);
imshow("Blur", imgBlur);
imshow("Canny", imgCanny);
imshow("Dil", imgDil);
*/
//cout << img.rows << endl;;
//display the orginal img with the contors drawn on to it
imshow("img", img);
// do the PID controller calculations
preData = PIDCONTROL(PosX);
//send the data to the arduino
data = intToString(preData);
cout << "Data =" << data << "." << endl;
char* charArray = new char[data.size() +1];
copy(data.begin(), data.end(), charArray);
charArray[data.size()] = '\n';
arduino.writeSerialPort(charArray, MAX_DATA_LENGTH);
arduino.readSerialPort(output, MAX_DATA_LENGTH);
cout << " Arduino >> " << output << "." << endl;
delete[] charArray;
waitKey(1); //work as fast as possible
// turns out this library can only output every second
}
}
Here is the Arduino code
#include <Servo.h>
Servo myservo;
String info;
void setup() {
Serial.begin(9600);
//pinMode(led,OUTPUT);
myservo.attach(9); // attaches the servo on pin 9 to the servo object
myservo.write(90); // set the starting position to 90 degrees(flat)
}
// this is the one for on using 10 degrees
void loop()
{
if (Serial.available() >0)
{
info = Serial.readStringUntil('\n');
}
Serial.flush();
//check what the imput is then change the servo level (every 10 degrees) and output the level
// checking the string other ways caused errors dunno why
if (info.equals("0"))
{
Serial.println("Not Receiving Data");
myservo.write(90);
}
else if (info.equals("80"))
{
Serial.println("80");
myservo.write(80);
}
else if (info.equals("81"))
{
Serial.println("81");
myservo.write(81);
}
else if (info.equals("82"))
{
Serial.println("82");
myservo.write(82);
}
else if (info.equals("83"))
{
Serial.println("83");
myservo.write(83);
}
else if (info.equals("84"))
{
Serial.println("84");
myservo.write(84);
}
else if (info.equals("85"))
{
Serial.println("85");
myservo.write(85);
}
else if (info.equals("86"))
{
Serial.println("86");
myservo.write(86);
}
else if (info.equals("87"))
{
Serial.println("87");
myservo.write(87);
}
else if (info.equals("88"))
{
Serial.println("88");
myservo.write(88);
}
else if (info.equals("89"))
{
Serial.println("89");
myservo.write(89);
}
else if (info.equals("90"))
{
Serial.println("90");
myservo.write(90);
}
else if (info.equals("91"))
{
Serial.println("91");
myservo.write(91);
}
else if (info.equals("92"))
{
Serial.println("92");
myservo.write(92);
}
else if (info.equals("93"))
{
Serial.println("93");
myservo.write(93);
}
else if (info.equals("94"))
{
Serial.println("94");
myservo.write(94);
}
else if (info.equals("95"))
{
Serial.println("95");
myservo.write(95);
}
else if (info.equals("96"))
{
Serial.println("96");
myservo.write(96);
}
else if (info.equals("97"))
{
Serial.println("97");
myservo.write(97);
}
else if (info.equals("98"))
{
Serial.println("98");
myservo.write(98);
}
else if (info.equals("99"))
{
Serial.println("99");
myservo.write(99);
}
else if (info.equals("100"))
{
Serial.println("100");
myservo.write(100);
}
}
Here is the headers file taken from https://www.youtube.com/watch?v=8BWjyZxGr5o
#ifndef SERIALPORT_H
#define SERIALPORT_H
#define ARDUINO_WAIT_TIME 2000
#define MAX_DATA_LENGTH 256
#include <windows.h>
#include <stdio.h>
#include <stdlib.h>
class SerialPort
{
private:
HANDLE handler;
bool connected;
COMSTAT status;
DWORD errors;
public:
SerialPort(char* portName);
~SerialPort();
int readSerialPort(char* buffer, unsigned int buf_size);
bool writeSerialPort(char* buffer, unsigned int buf_size);
bool isConnected();
};
#endif // SERIALPORT_H
#include "SerialPort.h"
SerialPort::SerialPort(char* portName)
{
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 {
DCB dcbSerialParameters = { 0 };
if (!GetCommState(this->handler, &dcbSerialParameters)) {
printf("failed to get current serial parameters");
}
else {
dcbSerialParameters.BaudRate = CBR_9600;
dcbSerialParameters.ByteSize = 8;
dcbSerialParameters.StopBits = ONESTOPBIT;
dcbSerialParameters.Parity = NOPARITY;
dcbSerialParameters.fDtrControl = DTR_CONTROL_ENABLE;
if (!SetCommState(handler, &dcbSerialParameters))
{
printf("ALERT: could not set Serial port parameters\n");
}
else {
this->connected = true;
PurgeComm(this->handler, PURGE_RXCLEAR | PURGE_TXCLEAR);
Sleep(ARDUINO_WAIT_TIME);
}
}
}
}
SerialPort::~SerialPort()
{
if (this->connected) {
this->connected = false;
CloseHandle(this->handler);
}
}
int SerialPort::readSerialPort(char* buffer, unsigned int buf_size)
{
DWORD bytesRead;
unsigned int toRead = 0;
ClearCommError(this->handler, &this->errors, &this->status);
if (this->status.cbInQue > 0) {
if (this->status.cbInQue > buf_size) {
toRead = buf_size;
}
else toRead = this->status.cbInQue;
}
if (ReadFile(this->handler, buffer, toRead, &bytesRead, NULL)) return bytesRead;
return 0;
}
bool SerialPort::writeSerialPort(char* buffer, unsigned int buf_size)
{
DWORD bytesSend;
if (!WriteFile(this->handler, (void*)buffer, buf_size, &bytesSend, 0)) {
ClearCommError(this->handler, &this->errors, &this->status);
return false;
}
else return true;
}
bool SerialPort::isConnected()
{
return this->connected;
}