void Balaenidae::doControl()
{
Controller c;
c.registers = myCopy;
c.registers.roll = 1;
if ((rand() % 100 + 1)<10)
c.registers.thrust = 20;
else
c.registers.thrust = 0;
doControl(c);
}
void MT_RobotFrameBase::doUserStep()
{
/* do tracking first, then control */
MT_TrackerFrameBase::doUserStep();
if(m_bAutoIdentify)
{
doAutoIdentify();
}
updateRobotStatesFromTracker();
doControl();
}
// this is executed continuously
void appBase::run() {
float idletime;
uint32_t thisLoop;
// delay loop to force update on even looptime boundaries
while ( millis() < nextLoop ) { // delay until time for next loop
if( /* lcd != NULL && */ buttons != NULL ) // check for button press
checkInput();
checkSerial(); // Has a command been received?
}
thisLoop = millis(); // actual time marker for this loop
// time stamp = system time, seconds, for this set of samples
timestamp = 0.001 * float( thisLoop ) - reftime;
getSamples(); // retrieve values from MCP9800 and MCP3424
if( first ) { // use first set of samples for RoR base values only
first = false;
initControl();
}
else {
logSamples(); // output results to serial port
doControl(); // perform control operations, if required
}
// save values for RoR calculation in next loop
for( int j = 0; j < _NCHAN; j++ ) {
int k = chan.getChan( j );
if( k != 0 ) {
--k;
flast[k] = ftemps[k];
lasttimes[k] = ftimes[k];
}
}
idletime = looptime - ( millis() - thisLoop );
// complain if we don't have some time left
if (idletime < _IDLE_TIME_ALARM ) {
Serial.print("# idle: ");
Serial.println(idletime);
}
nextLoop += looptime; // time mark for start of next update
// Serial.print("nextLoop="); Serial.print( nextLoop );
// Serial.print(" looptime="); Serial.print( looptime );
// Serial.println();
}
void Balaenidae::doControl(Controller controller)
{
doControl(controller.registers);
}
