Stap 5: 2 staat: uitvoeren deel 1
Het eerste deel van deze staat is een 2000ms vertraging. Dit werd gebruikt zodat de gebruiker de tijd gehad om hun hand uit de joystick te verwijderen voordat de robot beginnen zou te rijden.
Na dit, wordt de servo schrijven N kanaal blok gebruikt om de servomotoren vooruit door een pulsbreedte van de 2200uS aan de linker motor en 500uS pulse de juiste motor leveren. De puls breedte zijn verschillend, omdat een van de servo's is "achteruit."
Vanaf hier, een while lus wordt gebruikt om de kracht voelde en de waarde van de versnelling te controleren. Van het gegevensblad, de kracht van de force sensor kan worden benaderd voor lage krachten met behulp van de vergelijking F = 0.0389*exp(1.7727*V) waar F is de meting van de kracht en V is de spanning lezing. Een zaak structuur werd gebruikt zodat wanneer de spanning lezen over de kracht sensor nul is, de kracht lezing ook nul weergegeven. Om ervoor te zorgen kleine spanning fluctuaties zijn niet opgenomen in de lezing van kracht, wordt een select functie gebruikt zodat kleine spanningen leiden een lezing van de kracht van nul tot.
Het volgende idee was om te lezen van de kracht gemeten door de sensor en het display van de max waarde na de while lus werd overgenomen door waardoor het indexeren van de meting van de kracht en vervolgens met behulp van een min/max blokkeren om te lezen van de max waarde van de resulterende matrix.