skule.sormo.no

ORG NR 885 947 522

Arduinoprosjekter

Dreie kjøretøy mot kompasskurs

Denne programskissen styrer et kjøretøy mot en angitt kompasskurs.

Når ny kompasskurs blir gitt, må kjøretøye dreie enten mot venstre eller høyre , - fortrinnsvis den veien som er kortest, for å fortsette mot den nye kursen.

Kompasskursen oppgis ved hjelp av en IMU som har kompassmodul eller en enkelt HMC5883L-krets som kommuniserer ved hjelp av en I2C-bus. Kjøretøyet har da en en kurs som endres, og nå kjøretøyets retning endres mot den nye kursen. Dette skulle en tro er enkelt, men dersom ny kurs er 10 grader og gjeldende kurs er 340 grader, er det kortest å svinge til høyre, selv om å svinge til venstre reduserer kursen gradvis til 10 grader. Men da har kjøretøyet snurret unødvendig rundt for å komme på den nye kursen.

 

/* S: Sving, negativ mot klokka, poisitiv med klokka, -180 - +180.
   N: Ny kurs, 0-360 grader
   K: Aktuell kurs som skal korrigeres: 0-360 grader
*/

int K,N;

void SetSving(int S)
{
if (N > 180)
{
  if (K > N - 180)
    S=N - K; 
  else
{
   if (K < N - 180)
     S= - (K + 360 - N);
  else
    S = N - K;
 }
}
else
{
  if ((N + 180) > K)
    S = N - K;
  else
  {
     if (K < N)
        S = N - K;
     else
       S= 360 - K + N;   
   }
  }
}

 

Du har ikke rettigheter for å gi kommentarer