void TeleopPeriodic() { Camera(); DriveControl(); PrimeMotors(); Load(); HighGoal(); LowGoal(); ReadDistance(); UpdateServo(); }
void Slave_ProcessEvents(void) { ReadDistance(&distance); LED = !SWITCH; if(mRPDOIsGetRdy(1)) { mAppGoToPOSTMANEUVERstate(); mRPDORead(1); } if(limitMode && (APPLICATION_STATE == PREMANEUVER) ) { if((distance < limit) && !Hold && !TriggeredBySwitch) { TriggeredByDistance = 1; Hold = 1; mAppGoToMANEUVERstate(); //SEND PDO!!! if(mTPDOIsPutRdy(1)) mTPDOWritten(1); } else if(!SWITCH && !Hold && !TriggeredByDistance) { TriggeredBySwitch = 1; Hold = 1; mAppGoToMANEUVERstate(); //SEND PDO!!! if(mTPDOIsPutRdy(1)) mTPDOWritten(1); } if( (distance > limit) && TriggeredByDistance ) { Hold = 0; TriggeredByDistance = 0; } else if(SWITCH && TriggeredBySwitch) { Hold = 0; TriggeredBySwitch = 0; } } if(distanceMode) { if(mTPDOIsPutRdy(2)) mTPDOWritten(2); } }
void TeleopPeriodic() { ReadDistance(); }