Odmetry.cpp

00001 #include "Odmetry.hpp"
00002 
00003 Odmetry::Odmetry(){
00004     dt = 0.05;
00005     Radius = 395;
00006     Wradius = 25;
00007     XPos = 0;
00008     YPos = 0;
00009 }
00010 
00011 void Odmetry::pass_pulse(double PULSE1,double PULSE2,double PULSE3,double PULSE4){
00012     pulse_a[0] = PULSE1;
00013     pulse_a[1] = PULSE2;
00014     pulse_a[2] = PULSE3;
00015     pulse_a[3] = PULSE4;
00016     
00017     if(PULSE1 == PULSE2 == PULSE3 == PULSE4){
00018         return;
00019     }
00020     else{
00021         CalcSpd();
00022         Angular();
00023         transform();
00024         CalcPosition();
00025     }
00026 }
00027 
00028 void Odmetry::pass_angle(double THETA){
00029     Theta = THETA - PI / 4;
00030 }
00031     
00032 
00033 void Odmetry::CalcSpd(){
00034     for(int i = 0;i < 4;i ++){
00035         diff[i] = pulse_a[i] - pulse_b[i];
00036         spd[i] = Co * diff[i] / dt;
00037         pulse_b[i] = pulse_a[i];
00038     }
00039 }
00040 
00041 void Odmetry::Angular(){
00042     for(int i = 0;i < 4;i ++){
00043         Omega[i] = spd[i] / Radius;
00044     }
00045 }
00046 
00047 void Odmetry::transform(){
00048     Xpower = Wradius * (Omega[1] - Omega[3]);
00049     Ypower = Wradius * (Omega[0] - Omega[2]);
00050 }
00051 
00052 void Odmetry::CalcPosition(){
00053     Xspd = cos(Theta) * Xpower - sin(Theta) * Ypower;
00054     Yspd = sin(Theta) * Xpower + cos(Theta) * Ypower;
00055     
00056     XPos += Xspd * dt;
00057     YPos += Yspd * dt;
00058 }
00059 
00060 void Odmetry::print_Pos(){ printf("XPos:%lf YPos:%lf theta:%lf\n",XPos,YPos,Theta + PI / 2); }
00061 
00062 double Odmetry::obt_CurrentPosX(){ return XPos; }
00063 
00064 double Odmetry::obt_CurrentPosY(){ return YPos; }
00065 
00066 double Odmetry::obt_CurrentTheta(){ return Theta + PI / 2; }