Difference between revisions of "Trajectory from gyro/accellerometer/compas with 9DOF IMU (Michal Zemko, Peter Svitok)"
From RoboWiki
(→1. etapa) |
(→2. etapa) |
||
Line 28: | Line 28: | ||
== 2. etapa == | == 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); | ||
+ | } | ||
== 3. etapa == | == 3. etapa == |
Revision as of 15:58, 28 June 2013
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); }