Changeset 15 in flairsrc for trunk/lib/FlairFilter/src/TrajectoryGenerator2DCircle_impl.cpp
 Timestamp:
 04/08/16 15:40:57 (7 years ago)
 File:

 1 edited
Legend:
 Unmodified
 Added
 Removed

trunk/lib/FlairFilter/src/TrajectoryGenerator2DCircle_impl.cpp
r10 r15 31 31 using namespace flair::filter; 32 32 33 TrajectoryGenerator2DCircle_impl::TrajectoryGenerator2DCircle_impl(TrajectoryGenerator2DCircle* self,const LayoutPosition* position,string name) { 34 first_update=true; 35 is_running=false; 36 is_finishing=false; 33 TrajectoryGenerator2DCircle_impl::TrajectoryGenerator2DCircle_impl( 34 TrajectoryGenerator2DCircle *self, const LayoutPosition *position, 35 string name) { 36 first_update = true; 37 is_running = false; 38 is_finishing = false; 37 39 38 //init UI 39 GroupBox* reglages_groupbox=new GroupBox(position,name); 40 T=new DoubleSpinBox(reglages_groupbox>NewRow(),"period, 0 for auto"," s",0,1,0.01); 41 rayon=new DoubleSpinBox(reglages_groupbox>LastRowLastCol(),"R"," m",0,1000,.1); 42 veloctity=new DoubleSpinBox(reglages_groupbox>LastRowLastCol(),"velocity"," m/s",10,10,1); 43 acceleration=new DoubleSpinBox(reglages_groupbox>LastRowLastCol(),"acceleration (absolute)"," m/s²",0,10,.1); 40 // init UI 41 GroupBox *reglages_groupbox = new GroupBox(position, name); 42 T = new DoubleSpinBox(reglages_groupbox>NewRow(), "period, 0 for auto", " s", 43 0, 1, 0.01); 44 rayon = new DoubleSpinBox(reglages_groupbox>LastRowLastCol(), "R", " m", 0, 45 1000, .1); 46 veloctity = new DoubleSpinBox(reglages_groupbox>LastRowLastCol(), "velocity", 47 " m/s", 10, 10, 1); 48 acceleration = 49 new DoubleSpinBox(reglages_groupbox>LastRowLastCol(), 50 "acceleration (absolute)", " m/s²", 0, 10, .1); 44 51 45 //init matrix46 cvmatrix_descriptor* desc=new cvmatrix_descriptor(2,2);47 desc>SetElementName(0,0,"pos.x");48 desc>SetElementName(0,1,"pos.y");49 desc>SetElementName(1,0,"vel.x");50 desc>SetElementName(1,1,"vel.y");51 output=new cvmatrix(self,desc,floatType,name);52 // init matrix 53 cvmatrix_descriptor *desc = new cvmatrix_descriptor(2, 2); 54 desc>SetElementName(0, 0, "pos.x"); 55 desc>SetElementName(0, 1, "pos.y"); 56 desc>SetElementName(1, 0, "vel.x"); 57 desc>SetElementName(1, 1, "vel.y"); 58 output = new cvmatrix(self, desc, floatType, name); 52 59 53 output>SetValue(0,0,0);54 output>SetValue(0,1,0);55 output>SetValue(1,0,0);56 output>SetValue(1,1,0);60 output>SetValue(0, 0, 0); 61 output>SetValue(0, 1, 0); 62 output>SetValue(1, 0, 0); 63 output>SetValue(1, 1, 0); 57 64 } 58 65 59 60 66 TrajectoryGenerator2DCircle_impl::~TrajectoryGenerator2DCircle_impl() { 61 67 delete output; 62 68 } 63 69 64 void TrajectoryGenerator2DCircle_impl::StartTraj(const Vector2D &start_pos,float nb_lap) { 65 is_running=true; 66 first_update=true; 67 is_finishing=false; 68 this>nb_lap=nb_lap; 70 void TrajectoryGenerator2DCircle_impl::StartTraj(const Vector2D &start_pos, 71 float nb_lap) { 72 is_running = true; 73 first_update = true; 74 is_finishing = false; 75 this>nb_lap = nb_lap; 69 76 70 //configure trajectory71 angle_off=atan2(start_pos.ypos_off.y,start_pos.xpos_off.x);72 CurrentTime=0;77 // configure trajectory 78 angle_off = atan2(start_pos.y  pos_off.y, start_pos.x  pos_off.x); 79 CurrentTime = 0; 73 80 } 74 81 75 82 void TrajectoryGenerator2DCircle_impl::FinishTraj(void) { 76 if(!is_finishing) {77 is_finishing=true;78 FinishTime=CurrentTime;79 83 if (!is_finishing) { 84 is_finishing = true; 85 FinishTime = CurrentTime; 86 } 80 87 } 81 88 82 89 void TrajectoryGenerator2DCircle_impl::Update(Time time) { 83 84 85 float V=veloctity>Value();86 float A=acceleration>Value();87 float R=rayon>Value();88 90 float delta_t; 91 float theta; 92 float V = veloctity>Value(); 93 float A = acceleration>Value(); 94 float R = rayon>Value(); 95 Vector2D v; 89 96 90 if(V<0) A=A; 97 if (V < 0) 98 A = A; 91 99 92 if(T>Value()==0) { 93 if(first_update) { 94 first_update=false; 95 previous_time=time; 96 return; 100 if (T>Value() == 0) { 101 if (first_update) { 102 first_update = false; 103 previous_time = time; 104 return; 105 } else { 106 delta_t = (float)(time  previous_time) / 1000000000.; 107 } 108 } else { 109 delta_t = T>Value(); 110 } 111 previous_time = time; 112 CurrentTime += delta_t; 113 114 if (is_finishing && CurrentTime > FinishTime + V / A) 115 is_running = false; 116 117 if (is_running) { 118 if (R == 0) { 119 pos.x = 0; 120 pos.y = 0; 121 v.x = 0; 122 v.y = 0; 123 } else { 124 if (CurrentTime < V / A) { 125 theta = angle_off + A / 2 * CurrentTime * CurrentTime / R; 126 pos.x = R * cos(theta); 127 pos.y = R * sin(theta); 128 v.x = A * CurrentTime * sin(theta); 129 v.y = A * CurrentTime * cos(theta); 130 } else { 131 if (!is_finishing) { 132 theta = 133 angle_off + V * V / (2 * A * R) + (CurrentTime  V / A) * V / R; 134 pos.x = R * cos(theta); 135 pos.y = R * sin(theta); 136 v.x = V * sin(theta); 137 v.y = V * cos(theta); 97 138 } else { 98 delta_t=(float)(timeprevious_time)/1000000000.; 139 theta = angle_off + V * V / (2 * A * R) + 140 (FinishTime  V / A) * V / R  141 A / 2 * (FinishTime  CurrentTime) * 142 (FinishTime  CurrentTime) / R + 143 V * (CurrentTime  FinishTime) / R; 144 pos.x = R * cos(theta); 145 pos.y = R * sin(theta); 146 v.x = (V + A * (FinishTime  CurrentTime)) * sin(theta); 147 v.y = (V + A * (FinishTime  CurrentTime)) * cos(theta); 99 148 } 100 } else { 101 delta_t=T>Value(); 102 } 103 previous_time=time; 104 CurrentTime+=delta_t; 105 106 if(is_finishing && CurrentTime>FinishTime+V/A) is_running=false; 107 108 if(is_running) { 109 if(R==0) { 110 pos.x=0; 111 pos.y=0; 112 v.x=0; 113 v.y=0; 114 } else { 115 if(CurrentTime<V/A) { 116 theta=angle_off+A/2*CurrentTime*CurrentTime/R; 117 pos.x=R*cos(theta); 118 pos.y=R*sin(theta); 119 v.x=A*CurrentTime*sin(theta); 120 v.y=A*CurrentTime*cos(theta); 121 } else { 122 if(!is_finishing) { 123 theta=angle_off+V*V/(2*A*R)+(CurrentTimeV/A)*V/R; 124 pos.x=R*cos(theta); 125 pos.y=R*sin(theta); 126 v.x=V*sin(theta); 127 v.y=V*cos(theta); 128 } else { 129 theta=angle_off+V*V/(2*A*R)+(FinishTimeV/A)*V/RA/2*(FinishTimeCurrentTime)*(FinishTimeCurrentTime)/R+V*(CurrentTimeFinishTime)/R; 130 pos.x=R*cos(theta); 131 pos.y=R*sin(theta); 132 v.x=(V+A*(FinishTimeCurrentTime))*sin(theta); 133 v.y=(V+A*(FinishTimeCurrentTime))*cos(theta); 134 } 135 } 136 } 137 138 if(thetaangle_off>=nb_lap*2*PI(A/2*(V/A)*(V/A)/R+V*(V/A)/R) && nb_lap>0) { 139 FinishTraj(); 140 } 141 } else { 142 v.x=0; 143 v.y=0; 149 } 144 150 } 145 151 146 //on prend une fois pour toute les mutex et on fait des accès directs 147 output>GetMutex(); 148 output>SetValueNoMutex(0,0,pos.x+pos_off.x); 149 output>SetValueNoMutex(0,1,pos.y+pos_off.y); 150 output>SetValueNoMutex(1,0,v.x+vel_off.x); 151 output>SetValueNoMutex(1,1,v.y+vel_off.y); 152 output>ReleaseMutex(); 152 if (theta  angle_off >= nb_lap * 2 * PI  (A / 2 * (V / A) * (V / A) / R + 153 V * (V / A) / R) && 154 nb_lap > 0) { 155 FinishTraj(); 156 } 157 } else { 158 v.x = 0; 159 v.y = 0; 160 } 153 161 154 output>SetDataTime(time); 162 // on prend une fois pour toute les mutex et on fait des accès directs 163 output>GetMutex(); 164 output>SetValueNoMutex(0, 0, pos.x + pos_off.x); 165 output>SetValueNoMutex(0, 1, pos.y + pos_off.y); 166 output>SetValueNoMutex(1, 0, v.x + vel_off.x); 167 output>SetValueNoMutex(1, 1, v.y + vel_off.y); 168 output>ReleaseMutex(); 169 170 output>SetDataTime(time); 155 171 }
Note:
See TracChangeset
for help on using the changeset viewer.