Trajectory from gyro/accellerometer/compas

Zadanie

Trajectory from gyro/accellerometer/compas. Requirement: must use this part: 9DOF IMU, use any real robot.

O súčiastke

V projekte sme mali využiť súčiastku 9DOF IMU a následne zo získaných dát vykresliť prejdenú trajektóriu robota.

Na doske 9DOF IMU sa nachádzajú nasledujúce komponenty:

• akcelerometer
• magnetometer
• gyroskop

1. etapa

V prvej etape sme sa snažili prepojiť súčiastku s počítačom a získať z nej potrebné dáta. Postupným testovaním a dolaďovaním sme získali hodnoty:

• Yaw - udáva uhol zabočenia
• Pitch - udáva uhol stúpania / klesania
• Roll - udáva uhol výkyvu doprava / doľava

Nami získané údaje vyzerali takto:

2. etapa

Následne sme vykreslili dáta získané v prvej etape pomocou OpenGL:

  // pomocna konstanta
  float rozdiel_sipok = 12;
  
  // ********* YAW *********
  pushMatrix();

  translate(-700, 400);
  float angle = = radians(yaw - yawOffset);
  // sipka - smer dole => +180 smer hore
  rotateZ(angle + radians(180)); 
  line(0, -50, 0, 50);
  line(0, 50, rozdiel_sipok, 20);
  line(0, 50, -rozdiel_sipok, 20);

  popMatrix();
  
  // ********* PITCH *********
  pushMatrix();

  translate(-500, 400);
  rotateZ(radians(pitch));
  line(-50, 0, 50, 0);
  line(-50, 0, -50 + 20, rozdiel_sipok);
  line(-50, 0, -50 + 20, -rozdiel_sipok);
  
  popMatrix();
  
  // ********* ROLL *********
  pushMatrix();

  translate(-300, 400);
  rotateZ(radians(roll));
  line(-50, 0, 50, 0);
  
  popMatrix();

3. etapa

V tretej etape sme chceli dosiahnuť vykreslenie trajektórie podľa získaného Yaw (uhol zabočenia). S využitím polárnych súradníc a automatického pohybu vpred sme nakoniec dokázali vykresliť trajektóriu v rovine.

  angle = radians(yaw - yawOffset);
  // d - vzdialenosť, ktorú robot prejde
  double d = 0.9; 
  // deltaX, deltaZ - posun robota
  double deltaZ = cos(angle) * d;
  double deltaX = sin(angle) * d;
  // xx, yy = pozícia robota
  xx += deltaX;
  yy -= deltaZ;

Zdrojové kódy

projekt.pde