Trajectory from gyro/accellerometer/compas with 9DOF IMU (Michal Zemko, Peter Svitok)
From RoboWiki
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:
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); }