ericgeorge18
Posts: 1
Joined: Wed Jul 31, 2019 2:54 pm

Program Latency

Wed Jul 31, 2019 3:49 pm

Hello all!


I am having an issue with my programming having a minor lag when call one of the thread processes. It is extremely minor but enough that it isn't acceptable for the end result required. The device is using a Raspberry Pi 3 B+ to control a Roboclaw 2x30a motor controller. The end product of the whole unit spools wire from a large spool to smaller quantities onto another spool. The count and all of the other functions work perfectly without the traverse part active. The traverse is looking for an input on one of the gpio pins. When it is high, the traverse moves one way and when low the other allowing the wire to spool evenly. Every time this program activates to switch the motor direction, it misses a count. This only adds to about 7 missed counts in the span of 100 ft but needs to be within 2% for the tolerances we have. When the traverse is not active, the count is exactly where it needs to be. Is there anything in my code that I am missing that is causing this? I thought the Pi would have no problems running the main program along with 2 separate threads at the same time. I am running Rasbian Stretch Lite if that is a concern. I have included the whole code below for the main program. I am no expert at C++ and am sure some could have been cleaner but it is what it is and that is why I am asking for assistance. Thanks in advance with any help on this issue!

Code: Select all

#include <wiringPi.h>
#include "SimpleLCD.h"
#include <stdio.h>
#include <stdlib.h>
#include "roboclaw.h"
#include <string>
#include <cmath>
#include <iostream>
#include <fstream>
#include <ctime>
#include <pthread.h>

// stuff for count to work properly
int COUNT_KEY = 0;
int DEBOUNCE_TIME = 100;
static volatile int globalCounter = 0;

// setup the lcd pins
#define rs 2
#define strb 3
#define d4 4
#define d5 17
#define d6 27
#define d7 22

//constants
SimpleLCD lcd(rs, strb, d4, d5, d6, d7);
RoboClaw rc = RoboClaw();

int j = 0, t = 0, x = 127, y = 15, Confirm_Entry = 0, z = 0, k = 0, i = 0, b = 0;
int repeat = 1, peat = 1, stop = 0;
int counthold = 3, splicehold = 0, debouncetime = 0.5, q = 10, half = 0;
std::ofstream outfile;
time_t maxuphold1 = 0, maxdownhold1 = 0, maxuphold2 = 0, maxdownhold2 = 0;

PI_THREAD(waitForCount)
{
	unsigned int debounceTime = 0;
	
	(void)piHiPri(10); //set this thread to be high priority
	
	for (;;)
	{
		if (waitForInterrupt(23, -1) == 1)  // Got it  //int waitForInterrupt (int pin, int timeOut)
		{
			// Bouncing?
			if (millis() < debounceTime)
			{
				debounceTime = millis() + DEBOUNCE_TIME;
				continue;
			}

			piLock(COUNT_KEY);
			++globalCounter;
			piUnlock(COUNT_KEY);
		
			// Wait for key to be released
			while (digitalRead(23) == 1)
				delay(1);
	
			debounceTime = millis() + DEBOUNCE_TIME;
		}
	}
}


PI_THREAD(waitForTraverse)
{
	(void)piHiPri(5); //set this thread to be medium priority

	for (;;)
	{
		if (::stop == 0)
			continue;
				
		else if (::change_direction == 1)
		{
			if (::traverse_counter == 0)
			{
				rc.BackwardM1(127);
				::traverse_counter++;
			}
			else if (digitalRead(18) == 1 && ::traverse_counter > 25)
			{
				rc.BackwardM1(0);
				::traverse_counter = 0;
				::change_direction = 0;
				rc.ForwardM1(0);
			}
			else
				::traverse_counter++;
		}
		
		else
		{		
			if (::traverse_counter == 0)
			{
				rc.ForwardM1(127);
				::traverse_counter++;
			}
			else if (digitalRead(18) == 0 && ::traverse_counter > 25)
			{
				rc.ForwardM1(0);
				::traverse_counter = 0;
				::change_direction = 1;
				rc.BackwardM1(0);
			}
			else
				::traverse_counter++;

		}
	}
}

void DISPLAY()
{
	std::string bs = std::to_string(::b);
	const char *bsc = bs.c_str();
	std::string js = std::to_string(::j);
	const char *jsc = js.c_str();
	std::string ks = std::to_string(::k);
	const char *ksc = ks.c_str();
	std::string ts = std::to_string(::t);
	const char *tsc = ts.c_str();
	lcd.lcdClear();
	lcd.lcdWriteText("Count is:||Speed is:");
	lcd.lcdGoToRow(1);
	lcd.lcdWriteText(bsc);
	lcd.lcdWriteText("/");				
	lcd.lcdWriteText(jsc);
	lcd.lcdWriteText("||");
	lcd.lcdWriteText(ksc);
	lcd.lcdWriteText(" / ");				
	lcd.lcdWriteText(tsc);
}

void COUNT()
{
		::b = ::i;	

		//for quantities less than x - y feet
		if (::j <= (::x - ::y))
		{
			//speed up the take-up
			if (::i < (::j * .5))
			{
				if (::k < ::t)
					::k = ::k + 2;
				else //if (::k > ::t)
					::k = ::t;
			}
			//slow down the take-up
			else if (::i >= (::j - (((::k - ::y) * .5) + 5)))
			{
				::k = ::k - 2;
				//ensure the take-up doesn't go to zero before the count is reached
				if (::k < ::y)
					::k = ::y;
			}
			// make sure the speed wasn't changed while in limbo count land
			else
				::k = ::t;
		}

		//for quantities greater than x - y feet
		else if (::j > ((::x - ::y) - 1))
		{
			//speed up the take-up
			if (::i < (::j * .5))
			{
				if (::k < ::t)
					::k = ::k + 2;
				else if (::k > ::t)
					::k = ::t;
			}
			//slow down the take-up
			else if (::i > (::j - (::t * .5)))
			{
				::k = ::k - 2;
				if (::k < ::y)
					::k = ::y;
				else
					::k = ::t;
			}
			else if (::k > ::t)
				::k = ::t;
		}

		rc.ForwardM2(k);
}

void SPLICE()
{
	::stop = 0;
	::b = ::i;

	lcd.lcdClear();
	lcd.lcdWriteText("Wire Feed Error!");
	lcd.lcdGoToRow(1);
	lcd.lcdWriteText("START -> next screen");
}

void MAX_UP()
{
	if ((::maxuphold2 - ::maxuphold1) > ::debouncetime)
	{
		if (::t < ::x)
		{
			::t++;
			::maxuphold1 = ::maxuphold2 = 0;
		}
		else
		{
			lcd.lcdClear();
			::t = ::x;
			lcd.lcdWriteText("Max Speed!");
			::maxuphold1 = ::maxuphold2 = 0;
		}
	}
	else if (digitalRead(5) == 1)
		::maxuphold1 = ::maxuphold2 = 0;
	else
		::maxuphold2 = time(0);
}

void MAX_DOWN()
{
	if ((::maxdownhold2 - ::maxdownhold1) > ::debouncetime)
	{
		if (::t > ::y)
		{
			::t--;
			::maxdownhold1 = ::maxdownhold2 = 0;
		}
		else
		{
			lcd.lcdClear();
			::t = ::y;
			lcd.lcdWriteText("Min Speed!");
			::maxdownhold1 = ::maxdownhold2 = 0;
		}
	}
	else if (digitalRead(6) == 1)
		::maxdownhold1 = ::maxdownhold2 = 0;
	else
		::maxdownhold2 = time(0);
}

void STOP()
{
	::stop = 0;
	::b = ::i;

	lcd.lcdClear();
	lcd.lcdWriteText("You pressed STOP!");
	lcd.lcdGoToRow(1);
	lcd.lcdWriteText("START -> next screen");
}

void SETUP()
{
	//get quantity to count to
	while (TRUE)
	{
		while (TRUE)
		{
			while (TRUE)
			{
				lcd.lcdClear();
				lcd.lcdWriteText("Quantity?(1-9999)-->");
				std::cin >> ::j;
				if (std::cin.fail())
				{
					lcd.lcdClear();
					lcd.lcdWriteText("Input a whole number");
					lcd.lcdGoToRow(1);
					lcd.lcdWriteText("Lets try again....");
					std::cin.clear();
					std::cin.ignore(10, '\n');
					delay(1000);
				}
				else
				{
					std::cin.clear();
					std::cin.ignore(10, '\n');
					break;
				}
			}
			if (::j > 9999)
			{
				lcd.lcdClear();
				lcd.lcdWriteText("Quantity too high!");
				lcd.lcdGoToRow(1);
				lcd.lcdWriteText("Lets try again....");
				delay(1000);
			}
			else if (::j < 0)
			{
				lcd.lcdClear();
				lcd.lcdWriteText("Quantity too low!");
				lcd.lcdGoToRow(1);
				lcd.lcdWriteText("Lets try again....");
				delay(1000);
			}
			else if (::j > 0 && ::j < 9999)
				break;
			else if (::j == 0)
				delay(1);
			else
			{
				lcd.lcdClear();
				lcd.lcdWriteText("Lets try again....");
				delay(1000);
			}
		}

		//verify quantity
		while (TRUE)
		{
			lcd.lcdClear();
			lcd.lcdWriteText("You Entered ");
			std::string js = std::to_string(::j);
			const char *jsc = js.c_str();
			lcd.lcdWriteText(jsc);
			lcd.lcdGoToRow(1);
			lcd.lcdWriteText("Re-enter to confirm");
			std::cin >> Confirm_Entry;
			if (std::cin.fail())
			{
				lcd.lcdClear();
				lcd.lcdWriteText("Input a whole number");
				lcd.lcdGoToRow(1);
				lcd.lcdWriteText("Lets try again....");
				std::cin.clear();
				std::cin.ignore(10, '\n');
				delay(1000);
			}
			else
			{
				std::cin.clear();
				std::cin.ignore(10, '\n');
				break;
			}
		}
		if (Confirm_Entry != ::j)
		{
			lcd.lcdClear();
			lcd.lcdWriteText("Quantity mismatch!");
			lcd.lcdGoToRow(1);
			lcd.lcdWriteText("Lets try again....");
			delay(1000);
		}
		else
			break;
	}

	//set max speed of the take-up
	while (TRUE)
	{
		while (TRUE)
		{
			lcd.lcdClear();
			lcd.lcdWriteText("Set max speed.");
			lcd.lcdGoToRow(1);
			lcd.lcdWriteText("(1-127)-->");
			std::cin >> ::t;

			if (std::cin.fail())
			{
				std::cin.clear();
				std::cin.ignore(10, '\n');
				lcd.lcdClear();
				lcd.lcdWriteText("Input a whole number");
				lcd.lcdGoToRow(1);
				lcd.lcdWriteText("Lets try again....");
				delay(1000);
			}
			else
			{
				std::cin.clear();
				std::cin.ignore(10, '\n');
				break;
			}
		}
		if (::t > ::x)
		{
			lcd.lcdClear();
			lcd.lcdWriteText("Speed too high!");
			lcd.lcdGoToRow(1);
			lcd.lcdWriteText("Lets try again....");
			delay(1000);
		}
		else if (::t < ::y)
		{
			lcd.lcdClear();
			lcd.lcdWriteText("Speed too low!");
			lcd.lcdGoToRow(1);
			lcd.lcdWriteText("Lets try again....");
			delay(1000);
		}
		else
			break;
	}
}

void START()
{
	std::string ts = std::to_string(t);
	const char *tsc = ts.c_str();
	std::string js = std::to_string(j);
	const char *jsc = js.c_str();
	lcd.lcdClear();
	lcd.lcdWriteText("Count to ");
	lcd.lcdWriteText(jsc);
	lcd.lcdGoToRow(1);
	lcd.lcdWriteText("Max Speed is ");
	lcd.lcdWriteText(tsc);
	lcd.lcdGoToRow(2);
	lcd.lcdWriteText("Press START to begin");
	lcd.lcdGoToRow(3);
	lcd.lcdWriteText("RESET for new count.");

	while (TRUE)
	{
		if (digitalRead(12) == 0)
		{
			lcd.lcdClear();
			lcd.lcdWriteText("Start Spooling!");
			repeat = 3;
			break;
		}
		else if (digitalRead(26) == 0)
		{
			peat = 1;
			break;
		}
	}
}

void RC_SEEN()
{
	char version[512];
	int retry = 0;
	
	while (retry < 10)
	{
		if (rc.ReadVersion(version))
		{
			lcd.lcdClear();
			lcd.lcdWriteText("GETVERSION Passes!");
			lcd.lcdGoToRow(1);
			lcd.lcdWriteText("I see your Roboclaw!");
			break;
		}
		else
		{
			retry++;
			delay(100);
		}
	}
	
	if (!rc.ReadVersion(version))
	{
		lcd.lcdClear();
		lcd.lcdWriteText("GETVERSION Failed!");
		lcd.lcdGoToRow(1);
		lcd.lcdWriteText("Missing the Roboclaw");
		lcd.lcdGoToRow(2);
		lcd.lcdWriteText("RESTART to fix!!!");
		while (TRUE)
			delay(5000);
	}

	delay(500);
}

void SAVE_DATA()
{
	//save the data to a .CSV file
	time_t endnow = time(0);
	tm *endltm = localtime(&endnow);
	outfile << 1 + endltm->tm_mon << "," << endltm->tm_mday << "," << 1900 + endltm->tm_year << "," << endltm->tm_hour << "," << endltm->tm_min << "," << endltm->tm_sec << "," << b << "\n";
	std::cout << 1 + endltm->tm_mon << "," << endltm->tm_mday << "," << 1900 + endltm->tm_year << "," << endltm->tm_hour << "," << endltm->tm_min << "," << endltm->tm_sec << "," << b << "\n";
	outfile.close();
}

int main()
{
	// setup variables and initializing
	wiringPiSetupSys();  //initialize WiringPi for BCM numbering
	rc.begin(B19200);
	lcd.lcdInit();

	RC_SEEN();		// Check to make sure can see the roboclaw before continuing

	pinMode(23, INPUT);		//counter switch
	pullUpDnControl(23, PUD_DOWN);
	pinMode(12, INPUT);		//start button
	pullUpDnControl(12, PUD_UP);
	pinMode(26, INPUT);		//reset button
	pullUpDnControl(26, PUD_UP);
	pinMode(25, INPUT);		//stop button
	pullUpDnControl(25, PUD_UP);
	pinMode(24, INPUT);		//splice detector
	pullUpDnControl(24, PUD_UP);
	pinMode(5, INPUT);		//increase speed button
	pullUpDnControl(5, PUD_UP);
	pinMode(6, INPUT);		//decrease speed button
	pullUpDnControl(6, PUD_UP);
	pinMode(18, INPUT);		//traverse at front limit
	pullUpDnControl(18, PUD_UP);

	system("/usr/bin/gpio edge 23 falling");
	system("/usr/bin/gpio edge 18 both");
	piThreadCreate(waitForCount); //starts the interrupt handler
	piThreadCreate(waitForTraverse); //starts the interrupt handler

	//Reset starts back here
	while (TRUE)
	{
		while (::repeat == 1)
		{
			if (::peat == 1)
			{
				SETUP();
				::peat = 0;
			}
			else
				START();
		}
		
		::k = ::y, ::i = 0, ::b = 0, ::stop = 1, ::q = 10;
		piLock(COUNT_KEY);
		::globalCounter = 0;
		piUnlock(COUNT_KEY);

		//set time and open .csv file
		outfile.open("spool_log.csv", std::ios::app);
		time_t now = time(0);
		tm *ltm = localtime(&now);
		outfile << 1 + ltm->tm_mon << "," << ltm->tm_mday << "," << 1900 + ltm->tm_year << "," << ltm->tm_hour << "," << ltm->tm_min << "," << ltm->tm_sec << ",";
		std::cout << 1 + ltm->tm_mon << "," << ltm->tm_mday << "," << 1900 + ltm->tm_year << "," << ltm->tm_hour << "," << ltm->tm_min << "," << ltm->tm_sec << "," << std::endl;

		rc.ForwardM2(::k);

		while (TRUE)
		{
			if (::i != ::globalCounter)
			{
				piLock(COUNT_KEY);
				::i = ::globalCounter;
				piUnlock(COUNT_KEY);
				COUNT();
				if (::q >= 10)
				{
					::q = 0;
					DISPLAY();
				}
				else
					::q++;
			}
			else if (digitalRead(24) == 0 && ::splicehold == 0)
				::splicehold = 1;
			else if (::splicehold == 1)
			{
				SPLICE();
				::splicehold = 2;
			}
			else if ((digitalRead(5) == 0) && (::maxuphold1 == 0))
				::maxuphold1 = time(0);
			else if (::maxuphold1 != 0)
				MAX_UP();
			else if ((digitalRead(6) == 0) && (::maxdownhold1 == 0))
				::maxdownhold1 = time(0);
			else if (::maxdownhold1 != 0)
				MAX_DOWN();
			else if (digitalRead(25) == 0)
				STOP();
			else if (::stop == 0)
				break;
			else if (::b >= ::j)
			{
				lcd.lcdClear();
				lcd.lcdWriteText("START -> next screen");
				break;
			}
		}

		rc.flush();
		rc.ForwardM2(0);
 
		SAVE_DATA();
			
		while (TRUE)
		{
			rc.ForwardM2(0);
		
			if (digitalRead(12) == 0)
			{
				lcd.lcdClear();
				lcd.lcdWriteText("Completed ");
				std::string bs = std::to_string(::b);
				const char *bsc = bs.c_str();
				lcd.lcdWriteText(bsc);
				lcd.lcdWriteText(" feet.");
				lcd.lcdGoToRow(1);
				lcd.lcdWriteText("STOP for same count");	//25
				lcd.lcdGoToRow(2);
				lcd.lcdWriteText("RESET for new count");	//26
				break;
			}
		}

		while (TRUE)
		{
			if (digitalRead(26) == 0) //RESET
			{
				::peat = 1;
				break;
			}
			else if (digitalRead(25) == 0) //STOP
			{
				::peat = 0;
				break;
			}
		}

		while (TRUE)
		{
			if (digitalRead(24) == 0 && ::splicehold == 2)
			{
				lcd.lcdClear();
				lcd.lcdWriteText("Clear Wire Error");
				lcd.lcdGoToRow(1);
				lcd.lcdWriteText("Then Press RESET");
				::splicehold = 3;
			}
			else if (digitalRead(24) == 0)
				delay(1);
			else
			{
				while (digitalRead(26) == 1 && ::splicehold ==3)
					delay(1);
				break;
			}
		}
		::repeat = 1;
		::splicehold = 0;
		lcd.lcdClear();
	}

	return 0;
}

swampdog
Posts: 239
Joined: Fri Dec 04, 2015 11:22 am

Re: Program Latency

Fri Aug 02, 2019 11:27 am

I'm afraid, even with the source given, it is nigh on impossible to follow what is going on, certainly for me because I haven't done any gpio stuff yet. With that caveat in mind..

I can't see the mechanism for accessing what may be common variables across threads - ie: a mutex (except possibly globalCounter) and I'm dubious about changing thread priorities plus both threads seem to be in tight loops.

It looks like this pseudocode:

Code: Select all

ThreadA()
{
 //higher priority
 for (;;) ;
}

ThreadB()
{
 //lower priority
 for (;;) ;
}

main()
{
 StartThreadA
 StartThreadB
 for (;;) ;
..whereas the more expected form would be..

Code: Select all

ThreadA()
{
 for (;;) WaitForEvent;
}

ThreadB()
{
 for (;;) WaitForEvent;
}

main()
{
 StartThreadA
 StartThreadB
 for (;;) ;
}
..which might be achieved via main() polling the gpio (I think this is what you're doing) and using a ThreadA mutex and a ThreadB mutex (I can't see this if it is happening).

I'd hoped someone else who'd used wiringPI would comment because for all I know it may have hidden mechanisms which are doing this.

Return to “C/C++”