// need to check if all returns from here check for sys.stop
void maslowDelay(unsigned long waitTimeMs) {
/*
* Provides a time delay while holding the machine position, reading serial commands,
* and periodically sending the machine position to Ground Control. This prevents
* Ground Control from thinking that the connection is lost.
*
* This is similar to the pause() command above, but provides a time delay rather than
* waiting for the user (through Ground Control) to tell the machine to continue.
*/
unsigned long startTime = millis();
while ((millis() - startTime) < waitTimeMs){
execSystemRealtime();
if (sys.stop){return;}
}
}
// This should likely go away and be handled by setting the pause flag and then
// pausing in the execSystemRealtime function
// Need to check if all returns from this subsequently look to sys.stop
void pause(){
/*
The pause command pauses the machine in place without flushing the lines stored in the machine's
buffer.
When paused the machine enters a while() loop and doesn't exit until the '~' cycle resume command
is issued from Ground Control.
*/
bit_true(sys.pause, PAUSE_FLAG_USER_PAUSE);
Serial.println(F("Maslow Paused"));
while(bit_istrue(sys.pause, PAUSE_FLAG_USER_PAUSE)) {
// Run realtime commands
execSystemRealtime();
if (sys.stop){return;}
}
}
void Kinematics::forward(const float& chainALength, const float& chainBLength, float* xPos, float* yPos, float xGuess, float yGuess){
Serial.println(F("[Forward Calculating Position]"));
float guessLengthA;
float guessLengthB;
int guessCount = 0;
while(1){
//check our guess
inverse(xGuess, yGuess, &guessLengthA, &guessLengthB);
float aChainError = chainALength - guessLengthA;
float bChainError = chainBLength - guessLengthB;
//adjust the guess based on the result
xGuess = xGuess + .1*aChainError - .1*bChainError;
yGuess = yGuess - .1*aChainError - .1*bChainError;
guessCount++;
#if defined (KINEMATICSDBG) && KINEMATICSDBG > 0
Serial.print(F("[PEk:"));
Serial.print(aChainError);
Serial.print(',');
Serial.print(bChainError);
Serial.print(',');
Serial.print('0');
Serial.println(F("]"));
#endif
execSystemRealtime();
// No need for sys.stop check here
//if we've converged on the point...or it's time to give up, exit the loop
if((abs(aChainError) < .1 && abs(bChainError) < .1) or guessCount > KINEMATICSMAXGUESS or guessLengthA > sysSettings.chainLength or guessLengthB > sysSettings.chainLength){
if((guessCount > KINEMATICSMAXGUESS) or guessLengthA > sysSettings.chainLength or guessLengthB > sysSettings.chainLength){
Serial.print(F("Message: Unable to find valid machine position for chain lengths "));
Serial.print(chainALength);
Serial.print(", ");
Serial.print(chainBLength);
Serial.println(F(" . Please set the chains to a known length (Actions -> Set Chain Lengths)"));
*xPos = 0;
*yPos = 0;
}
else{
Serial.println("position loaded at:");
Serial.println(xGuess);
Serial.println(yGuess);
*xPos = xGuess;
*yPos = yGuess;
}
break;
}
}
}
