Trajectory from gyro/accellerometer/compas with 9DOF IMU (Michal Zemko, Peter Svitok)

Zadanie

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

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

Etapy

• 1. Oboznamánie sa s projektom
• 2. Naprogramovanie v Jave
• 3. Záverečné prípravy

1. etapa

V prvej etape sme dostali súčiastku a cez program Putty skúšali jej funkčnosť.

2. etapa

Pohyb do ľubovoľného smeru

void move(int angle,int speed,int time)
{
int uhol=0;
float zvislo = 0.0;
float vodorovne = 0.0;


if (angle>=0 && angle <90){
uhol = angle;
zvislo = (90.0-uhol)/90.0*speed; // vypocet percent akou rychlostou sa tocia motory v smere x-ovej osi
vodorovne = uhol/90.0*speed;  // vypocet percent akou rychlostou sa tocia motory v smere y-ovej osi
int z = zvislo; 
int v = vodorovne;
setMotor(3,z,1);
setMotor(2,z,1);
setMotor(1,v,1);
setMotor(4,v,0);

wait(time);
setMotor(1, 0, 0);
setMotor(2, 0, 0);
setMotor(3, 0, 0);
setMotor(4, 0, 0);
}

if (angle>=90 && angle <180){

uhol = angle-90;
zvislo = (90.0-uhol)/90.0*speed;
vodorovne = uhol/90.0*speed;
int z = zvislo;
int v = vodorovne;

setMotor(3,v,0);
setMotor(2,v,0);
setMotor(1,z,1);
setMotor(4,z,0);
wait(time);
setMotor(1, 0, 0);
setMotor(2, 0, 0);
setMotor(3, 0, 0);
setMotor(4, 0, 0);
}

if (angle>=180 && angle <270){

uhol = angle-180;
zvislo = (90.0-uhol)/90.0*speed;
vodorovne = uhol/90.0*speed;
int z = zvislo;
int v = vodorovne;

setMotor(3,z,0);
setMotor(2,z,0);
setMotor(1,v,0);
setMotor(4,v,1);
wait(time);
setMotor(1, 0, 0);
setMotor(2, 0, 0);
setMotor(3, 0, 0);
setMotor(4, 0, 0);
}

if (angle>=270 && angle <=360){

uhol = angle-270;
zvislo = (90.0-uhol)/90.0*speed;
vodorovne = uhol/90.0*speed;
int z = zvislo;
int v = vodorovne;

setMotor(3,v,1);
setMotor(2,v,1);
setMotor(1,z,0);
setMotor(4,z,1);
wait(time);
setMotor(1, 0, 0);
setMotor(2, 0, 0);
setMotor(3, 0, 0);
setMotor(4, 0, 0);
}

Rotácie

void RotateLeft(int speed, int time)
{
setMotor(1,speed,0);
setMotor(2,speed,1);
setMotor(3,speed,0);
setMotor(4,speed,0);
wait(time);
setMotor(1, 0, 0);
setMotor(2, 0, 0);
setMotor(3, 0, 0);
setMotor(4, 0, 0);
}

void RotateRight(int speed, int time)
{
setMotor(1,speed,1);
setMotor(2,speed,0);
setMotor(3,speed,1);
setMotor(4,speed,1);
wait(time);
setMotor(1, 0, 0);
setMotor(2, 0, 0);
setMotor(3, 0, 0);
setMotor(4, 0, 0);
}