Here you have.
////////////// ABSTRACT ////////////////////////
// This piece of code is an excerpt of a program to control a hobby car. Roughly: it is a function that, when -continuosly- called from the main loop, adjusts the speed of the motor to the desired one (AMSpeedDesired).
// The code works fine. As it has been posted just to show how to program a "finite state automat", the global constants and variables definitions, the setup and the loop code have been ommited.
// This is the code as it is in my program. Some of the code has to be improved -removing values and substituing them for variables, piecing the code into "subfunctions" to make it a little bit more readable, etcetera-. For the sake of clarity I've added some comments on the fly (the whole program is "half" commented). As it is my first "serious" program in "arduino C" you may find some inconsistencies (type of variables, mainly). Sorry for that.
// V. Fombellida (vffgaston). July 2014.
////////// Functioning of the program //////////////
// A "finite state automat" is a technique to code programs that have a complicated flow and/or have to attend "events" that appear randomly. You have a short explanation in the wikipedia (the example it shows is, in my opinion, too simple as to make oneself a complete idea on the power of the technique).
// In this example the DC motor can be in five different states ("AMStatus") that correspond to "Stopped",. "Controlled -Ramp- Start", "Feed back controlled -proportional control-", "Restart -from an abnormal stop-" and "Unconditional -not controlled- start". In addition to the normal "Current state -> Event -> Transition -> Next state" logic, in this example one out of five pieces of code -corresponding to the five states- is executed every time the function is called.
// The speed (AMSpeed) is obtained by calculating the inverse of the interval corresponding to the RISING edge of the last two teeth of the encoder. For this purpose (storing the times) an isr routine is executed every RISING edge.
// The automat table is stored in four arrays:
// const int AMCCCStatus[] = {0,0,1,1,2,2,3,3,4};
// const int AMCC__Event[] = {4,2,3,1,3,0,3,1,5};
// const int AMCCNStatus[] = {4,1,0,2,0,3,0,2,2};
// const int AMCCTrnston[] = {5,1,2,3,2,4,2,3,6};
// (On the analysis phase you will write this table with 9 rows 0-4-4-5 ....... 4-5-2-6 and 4 columns; I've coded it this way because it seems much easier to write the rest of the code -to search for a matching event, determine transition, etcetera-).
// Of course, the amount of rows on a particular case -and their content- is determined by your specific problem. Any case, the rest of the code (to look for the matching event, next state and corresponding transition) should be valid for any implementation.
// In this particular case, the events are all "internal" in the sense that all of them are generated by times exceded and the other parts of the code changing the "AMDesiredSpeed". Just the event "1" can be deemed as an external one (new tooth).
// Just to show the power of this technique: I added the state number 4 (the fifth, in fact) because the starting of those hobby motors is almost completely random (for the same DC voltage the same motor can actually start or it can decide to wait some hundredths of volt; to make sure it actually starts better to give it full voltage for a while). As, for this purpose, the code is trivial ("analogWrite(AMotPWM , 255);") the modification of the program flow (adding a line to the table) took few minutes.
/////////////////// A_M_Ctrl_Code /////////////////////
void A_M_Ctrl_Code()
{
// Specific variables and constants
int i = 0;
volatile float AMSCorrection = 0;
volatile float AMHeave = 0;
volatile float AMSpeedError = 0;
const float AMK = 40;
const int AMHeaveProp = 10;
const float AMSpeedCorrFactor = 0.25;
const float AMUnderSpeed = 4.0;
const float AMFxdStTime = 600;
/////////////////////////////////////////////////////
// Automat table
const int AMCCLines = 9;
const int AMCCCStatus[] = {0,0,1,1,2,2,3,3,4};
const int AMCC__Event[] = {4,2,3,1,3,0,3,1,5};
const int AMCCNStatus[] = {4,1,0,2,0,3,0,2,2};
const int AMCCTrnston[] = {5,1,2,3,2,4,2,3,6};
// Other automat variables
boolean AMCCUnattendedEvent = false;
int AMCCTransitionToExecute;
// Events updating
boolean AMCCEvents[6] = {0,0,0,0,0,0};
if (( float (millis() - AMSpCalCurTime)/1000.0) > (AMUnderSpeed / AMSpeedDesired))
{
AMCCEvents[0] = true;
}
else
{
AMCCEvents[0] = false;
}
if (AMSpCalPrvTime < AMSpCalCurTime)
{
AMCCEvents[1] = true;
}
else
{
AMCCEvents[1] = false;
}
if (AMSpeedDesired != 0)
{
AMCCEvents[2] = true;
AMCCEvents[3] = false;
}
else
{
AMCCEvents[3] = true;
AMCCEvents[2] = false;
}
if (millis() <= AMFxdStTime)
{
AMCCEvents[4] = true;
AMCCEvents[5] = false;
}
else
{
AMCCEvents[5] = true;
AMCCEvents[4] = false;
}
// Look for suitable event (corresponding to the current state)
while (AMStatus >= AMCCCStatus[i] && i <= AMCCLines)
{
if (AMStatus == AMCCCStatus[i] && (AMCCEvents[AMCC__Event[i]]))
{
AMStatus = AMCCNStatus[i]; // Update state
AMCCUnattendedEvent = true; // Flag for transitions
AMCCTransitionToExecute = AMCCTrnston[i];
}
i++;
}
//Transitions managing
if (AMCCUnattendedEvent)
{
switch (AMCCTransitionToExecute)
{
case 0:
{
AMCCUnattendedEvent = false;
}
break;
case 1:
{
AMCCUnattendedEvent = false;
}
break;
case 2:
{
AMStatus = 0;
AMCCUnattendedEvent = false;
}
case 3:
{
AMCCUnattendedEvent = false;
}
case 4:
{
AMCCUnattendedEvent = false;
}
break;
case 5:
{
AMCCUnattendedEvent = false;
}
break;
}
}
//Code to execute.
switch (AMStatus)
{
case 0:
{
analogWrite(AMotPWM , 0);
AMDCOutputPrv = 0;
}
break;
case 1:
{
AMHeave = AMK + AMSpeedDesired * AMHeaveProp * ((millis() - (AMSpCalCurTime))/ 1000.0);
if (AMHeave > 255.0) AMHeave = 255.0;
if (AMHeave < 0.0) AMHeave = 0.0;
AMDCOutputNew = AMHeave;
AMDCOutputPrv = AMDCOutputNew;
analogWrite(AMotPWM , AMDCOutputNew);
}
break;
case 2:
{
if (AMSpCalPrvTime < AMSpCalCurTime)
{
AMSpeed = 1000.0/(AMSpCalCurTime - AMSpCalPrvTime);
AMSpeedError = AMSpeedDesired - AMSpeed;
AMSCorrection = AMSpeedCorrFactor * AMSpeedError;
AMDCOutputNew = AMDCOutputPrv + AMSCorrection;
if (AMDCOutputNew < 0) AMDCOutputNew = 0;
if (AMDCOutputNew > 255) AMDCOutputNew = 255;
analogWrite(AMotPWM , AMDCOutputNew);
AMDCOutputPrv = AMDCOutputNew;
AMSpCalPrvTime = AMSpCalCurTime;
}
}
break;
case 3:
{
AMHeave = AMK + AMSpeedDesired * (AMHeaveProp * ((millis() - AMSpCalCurTime)/ 1000.0) - (AMUnderSpeed / AMSpeedDesired));
if (AMHeave > 255.0) AMHeave = 255.0;
AMDCOutputNew = AMHeave;
if (AMDCOutputNew > 255.0) AMDCOutputNew = 255.0;
if (AMDCOutputNew < 0) AMDCOutputNew = 0;
AMDCOutputPrv = AMDCOutputNew;
analogWrite(AMotPWM , AMDCOutputNew);
}
break;
case 4:
{
analogWrite(AMotPWM , 255);
}
break;
}
}
//////////////// A_M_Ctrl_Code end //////////////////////
(Just to make sure: I adjoint the sketch I used to code the table)
Regards
140728_EXAMPLE.pdf (628 KB)
