Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Newer
Older
100644 340 lines (258 sloc) 7.757 kB
841bf27 added simple power model and some cosmetic and gui fixes
rtv authored
1 /** powerpack.cc
2 Simple model of energy storage
3 Richard Vaughan
4 Created 2009.1.15
639ffa9 added revision keyword props to all source files
rtv authored
5 SVN: $Id$
841bf27 added simple power model and some cosmetic and gui fixes
rtv authored
6 */
7
ec56d04 header clean up
rtv authored
8 #include "stage.hh"
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
9 #include "texture_manager.hh"
ec56d04 header clean up
rtv authored
10 using namespace Stg;
841bf27 added simple power model and some cosmetic and gui fixes
rtv authored
11
203687c working on energy monitoring
rtv authored
12
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
13 joules_t PowerPack::global_stored = 0.0;
14 joules_t PowerPack::global_input = 0.0;
15 joules_t PowerPack::global_capacity = 0.0;
16 joules_t PowerPack::global_dissipated = 0.0;
203687c working on energy monitoring
rtv authored
17
841bf27 added simple power model and some cosmetic and gui fixes
rtv authored
18 PowerPack::PowerPack( Model* mod ) :
5402eed bugfix, fasr2
rtv authored
19 event_vis( 2.0 * std::max( fabs(ceil(mod->GetWorld()->GetExtent().x.max)),
20 fabs(floor(mod->GetWorld()->GetExtent().x.min))),
21 2.0 * std::max( fabs(ceil(mod->GetWorld()->GetExtent().y.max)),
22 fabs(floor(mod->GetWorld()->GetExtent().y.min))),
23 1.0 ),
4ccc5cd replaced stg_color_t with Color class and added static named construc…
rtv authored
24 output_vis( 0,100,200,40, 1200, Color(1,0,0), Color(0,0,0,0.5), "energy output", "energy_input" ),
25 stored_vis( 0,142,200,40, 1200, Color(0,1,0), Color(0,0,0,0.5), "energy stored", "energy_stored" ),
203687c working on energy monitoring
rtv authored
26 mod( mod),
27 stored( 0.0 ),
28 capacity( 0.0 ),
4a35091 added missing initializer in powerpack
rtv authored
29 charging( false ),
5ab3af7 added vis of current draw from powerpacks
rtv authored
30 dissipated( 0.0 ),
31 last_time(0),
32 last_joules(0.0),
33 last_watts(0.0)
841bf27 added simple power model and some cosmetic and gui fixes
rtv authored
34 {
e65967f powerpacks now set charging flag correctly
rtv authored
35 // tell the world about this new pp
203687c working on energy monitoring
rtv authored
36 mod->world->AddPowerPack( this );
3777995 adding strip plot vis
rtv authored
37
427aa67 working on power models
rtv authored
38 mod->AddVisualizer( &event_vis, false );
5402eed bugfix, fasr2
rtv authored
39 mod->AddVisualizer( &output_vis, false );
40 mod->AddVisualizer( &stored_vis, false );
3777995 adding strip plot vis
rtv authored
41 }
841bf27 added simple power model and some cosmetic and gui fixes
rtv authored
42
e65967f powerpacks now set charging flag correctly
rtv authored
43 PowerPack::~PowerPack()
44 {
45 mod->world->RemovePowerPack( this );
203687c working on energy monitoring
rtv authored
46 mod->RemoveVisualizer( &event_vis );
3777995 adding strip plot vis
rtv authored
47 mod->RemoveVisualizer( &output_vis );
48 mod->RemoveVisualizer( &stored_vis );
e65967f powerpacks now set charging flag correctly
rtv authored
49 }
50
841bf27 added simple power model and some cosmetic and gui fixes
rtv authored
51 /** OpenGL visualization of the powerpack state */
5ab3af7 added vis of current draw from powerpacks
rtv authored
52 void PowerPack::Visualize( Camera* cam )
841bf27 added simple power model and some cosmetic and gui fixes
rtv authored
53 {
3ce2d0e applied patch from Jeff Donner, fixing some signed/unsigned comparisons
rtv authored
54 (void)cam; // avoid warning about unused var
55
841bf27 added simple power model and some cosmetic and gui fixes
rtv authored
56 const double height = 0.5;
d15da18 added simple charger and visualizations
rtv authored
57 const double width = 0.2;
841bf27 added simple power model and some cosmetic and gui fixes
rtv authored
58
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
59 double percent = stored/capacity * 100.0;
e65967f powerpacks now set charging flag correctly
rtv authored
60
ff0dbdd working on charging model and demo
rtv authored
61 const double alpha = 0.5;
62
e65967f powerpacks now set charging flag correctly
rtv authored
63 if( percent > 50 )
64 glColor4f( 0,1,0, alpha ); // green
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
65 else if( percent > 25 )
ff0dbdd working on charging model and demo
rtv authored
66 glColor4f( 1,0,1, alpha ); // magenta
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
67 else
ff0dbdd working on charging model and demo
rtv authored
68 glColor4f( 1,0,0, alpha ); // red
e65967f powerpacks now set charging flag correctly
rtv authored
69
5ab3af7 added vis of current draw from powerpacks
rtv authored
70 // snprintf( buf, 32, "%.0f", percent );
e65967f powerpacks now set charging flag correctly
rtv authored
71
72 glTranslatef( -width, 0.0, 0.0 );
73
74 glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
75
76 GLfloat fullness = height * (percent * 0.01);
77 glRectf( 0,0,width, fullness);
78
79 // outline the charge-o-meter
165fe8b fixed energy dissipation vis bug
rtv authored
80 glTranslatef( 0,0,0.1 );
e65967f powerpacks now set charging flag correctly
rtv authored
81 glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
82
83 glColor4f( 0,0,0,0.7 );
84
85 glRectf( 0,0,width, height );
86
87 glBegin( GL_LINES );
88 glVertex2f( 0, fullness );
89 glVertex2f( width, fullness );
90 glEnd();
91
92 if( stored < 0.0 ) // inifinite supply!
93 {
94 // draw an arrow toward the top
95 glBegin( GL_LINES );
96 glVertex2f( width/3.0, height/3.0 );
97 glVertex2f( 2.0 * width/3, height/3.0 );
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
98
e65967f powerpacks now set charging flag correctly
rtv authored
99 glVertex2f( width/3.0, height/3.0 );
100 glVertex2f( width/3.0, height - height/5.0 );
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
101
e65967f powerpacks now set charging flag correctly
rtv authored
102 glVertex2f( width/3.0, height - height/5.0 );
103 glVertex2f( 0, height - height/5.0 );
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
104
e65967f powerpacks now set charging flag correctly
rtv authored
105 glVertex2f( 0, height - height/5.0 );
106 glVertex2f( width/2.0, height );
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
107
e65967f powerpacks now set charging flag correctly
rtv authored
108 glVertex2f( width/2.0, height );
109 glVertex2f( width, height - height/5.0 );
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
110
e65967f powerpacks now set charging flag correctly
rtv authored
111 glVertex2f( width, height - height/5.0 );
112 glVertex2f( 2.0 * width/3.0, height - height/5.0 );
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
113
e65967f powerpacks now set charging flag correctly
rtv authored
114 glVertex2f( 2.0 * width/3.0, height - height/5.0 );
115 glVertex2f( 2.0 * width/3, height/3.0 );
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
116
e65967f powerpacks now set charging flag correctly
rtv authored
117 glEnd();
118 }
119
841bf27 added simple power model and some cosmetic and gui fixes
rtv authored
120
e65967f powerpacks now set charging flag correctly
rtv authored
121 if( charging )
122 {
123 glLineWidth( 6.0 );
124 glColor4f( 1,0,0,0.7 );
125
126 glRectf( 0,0,width, height );
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
127
e65967f powerpacks now set charging flag correctly
rtv authored
128 glLineWidth( 1.0 );
129 }
130
5ab3af7 added vis of current draw from powerpacks
rtv authored
131
132 // compute the instantaneous power output
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
133 usec_t time_now = mod->world->SimTimeNow();
134 usec_t delta_t = time_now - last_time;
135 watts_t watts = last_watts;
5ab3af7 added vis of current draw from powerpacks
rtv authored
136
137 if( delta_t > 0 ) // some sim time elapsed
138 {
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
139 joules_t delta_j = stored - last_joules;
140 watts_t watts = (-1e6 * delta_j) / (double)delta_t;
5ab3af7 added vis of current draw from powerpacks
rtv authored
141
142 last_joules = stored;
143 last_time = time_now;
144 last_watts = watts;
145 }
146
147 if( fabs(watts) > 1e-5 ) // any current
148 {
149 glColor4f( 1,0,0,0.8 ); // red
150 char buf[32];
151 snprintf( buf, 32, "%.1fW", watts );
152 Gl::draw_string( -0.05,height+0.05,0, buf );
153 }
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
154 }
841bf27 added simple power model and some cosmetic and gui fixes
rtv authored
155
156
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
157 joules_t PowerPack::RemainingCapacity() const
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
158 {
159 return( capacity - stored );
160 }
d15da18 added simple charger and visualizations
rtv authored
161
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
162 void PowerPack::Add( joules_t j )
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
163 {
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
164 joules_t amount = std::min( RemainingCapacity(), j );
203687c working on energy monitoring
rtv authored
165 stored += amount;
166 global_stored += amount;
167
168 if( amount > 0 ) charging = true;
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
169 }
841bf27 added simple power model and some cosmetic and gui fixes
rtv authored
170
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
171 void PowerPack::Subtract( joules_t j )
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
172 {
203687c working on energy monitoring
rtv authored
173 if( stored < 0 ) // infinte supply!
174 {
175 global_input += j; // record energy entering the system
176 return;
177 }
6e109cd fixed bug in energy dissiparion model
rtv authored
178
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
179 joules_t amount = std::min( stored, j );
203687c working on energy monitoring
rtv authored
180
181 stored -= amount;
182 global_stored -= amount;
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
183 }
841bf27 added simple power model and some cosmetic and gui fixes
rtv authored
184
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
185 void PowerPack::TransferTo( PowerPack* dest, joules_t amount )
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
186 {
ff0dbdd working on charging model and demo
rtv authored
187 //printf( "amount %.2f stored %.2f dest capacity %.2f\n",
188 // amount, stored, dest->RemainingCapacity() );
189
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
190 // if stored is non-negative we can't transfer more than the stored
191 // amount. If it is negative, we have infinite energy stored
192 if( stored >= 0.0 )
910541e removed glib2 dependency (after many years of good service. Thanks GL…
rtv authored
193 amount = std::min( stored, amount );
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
194
195 // we can't transfer more than he can take
910541e removed glib2 dependency (after many years of good service. Thanks GL…
rtv authored
196 amount = std::min( amount, dest->RemainingCapacity() );
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
197
ff0dbdd working on charging model and demo
rtv authored
198 //printf( "%s gives %.3f J to %s\n",
199 // mod->Token(), amount, dest->mod->Token() );
83a7a8d added contact recharging between power packs. Removed charger object
rtv authored
200
201 Subtract( amount );
202 dest->Add( amount );
9008cbf fixed issue where powerpack charge changes were not always drawnin th…
rtv authored
203
204 mod->NeedRedraw();
841bf27 added simple power model and some cosmetic and gui fixes
rtv authored
205 }
203687c working on energy monitoring
rtv authored
206
207
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
208 void PowerPack::SetCapacity( joules_t cap )
203687c working on energy monitoring
rtv authored
209 {
210 global_capacity -= capacity;
211 capacity = cap;
212 global_capacity += capacity;
213
214 if( stored > cap )
215 {
216 global_stored -= stored;
217 stored = cap;
218 global_stored += stored;
219 }
220 }
221
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
222 joules_t PowerPack::GetCapacity() const
203687c working on energy monitoring
rtv authored
223 {
224 return capacity;
225 }
226
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
227 joules_t PowerPack::GetStored() const
203687c working on energy monitoring
rtv authored
228 {
229 return stored;
230 }
231
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
232 joules_t PowerPack::GetDissipated() const
b96211c added more powerpack API
rtv authored
233 {
234 return dissipated;
235 }
236
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
237 void PowerPack::SetStored( joules_t j )
203687c working on energy monitoring
rtv authored
238 {
239 global_stored -= stored;
240 stored = j;
241 global_stored += stored;
242 }
243
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
244 void PowerPack::Dissipate( joules_t j )
203687c working on energy monitoring
rtv authored
245 {
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
246 joules_t amount = (stored < 0) ? j : std::min( stored, j );
203687c working on energy monitoring
rtv authored
247
248 Subtract( amount );
b96211c added more powerpack API
rtv authored
249 dissipated += amount;
203687c working on energy monitoring
rtv authored
250 global_dissipated += amount;
427aa67 working on power models
rtv authored
251
3777995 adding strip plot vis
rtv authored
252 output_vis.AppendValue( amount );
253 stored_vis.AppendValue( stored );
203687c working on energy monitoring
rtv authored
254 }
255
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
256 void PowerPack::Dissipate( joules_t j, const Pose& p )
203687c working on energy monitoring
rtv authored
257 {
258 Dissipate( j );
427aa67 working on power models
rtv authored
259 event_vis.Accumulate( p.x, p.y, j );
203687c working on energy monitoring
rtv authored
260 }
261
262 //------------------------------------------------------------------------------
263 // Dissipation Visualizer class
264
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
265 joules_t PowerPack::DissipationVis::global_peak_value = 0.0;
427aa67 working on power models
rtv authored
266
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
267 PowerPack::DissipationVis::DissipationVis( meters_t width,
268 meters_t height,
269 meters_t cellsize )
203687c working on energy monitoring
rtv authored
270 : Visualizer( "energy dissipation", "energy_dissipation" ),
427aa67 working on power models
rtv authored
271 columns(width/cellsize),
272 rows(height/cellsize),
273 width(width),
274 height(height),
602b385 energy dissipation vis now uses vector instead of array
rtv authored
275 cells( columns*rows ),
427aa67 working on power models
rtv authored
276 peak_value(0),
277 cellsize(cellsize)
203687c working on energy monitoring
rtv authored
278 { /* nothing to do */ }
427aa67 working on power models
rtv authored
279
280 PowerPack::DissipationVis::~DissipationVis()
281 {
282 }
203687c working on energy monitoring
rtv authored
283
284 void PowerPack::DissipationVis::Visualize( Model* mod, Camera* cam )
285 {
3ce2d0e applied patch from Jeff Donner, fixing some signed/unsigned comparisons
rtv authored
286 (void)cam; // avoid warning about unused var
287
203687c working on energy monitoring
rtv authored
288 // go into world coordinates
289
290 glPushMatrix();
427aa67 working on power models
rtv authored
291
59d66b4 debugging rasterization
rtv authored
292 Gl::pose_inverse_shift( mod->GetGlobalPose() );
427aa67 working on power models
rtv authored
293
82dca0d added callbacks for flag push and pop, and vis improvements
rtv authored
294 glTranslatef( -width/2.0, -height/2.0, 0.01 );
427aa67 working on power models
rtv authored
295 glScalef( cellsize, cellsize, 1 );
296
297 for( unsigned int y=0; y<rows; y++ )
298 for( unsigned int x=0; x<columns; x++ )
299 {
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
300 joules_t j = cells[ y*columns + x ];
165fe8b fixed energy dissipation vis bug
rtv authored
301
302 //printf( "%d %d %.2f\n", x, y, j );
303
427aa67 working on power models
rtv authored
304 if( j > 0 )
305 {
165fe8b fixed energy dissipation vis bug
rtv authored
306 glColor4f( 1.0, 0, 0, j/global_peak_value );
427aa67 working on power models
rtv authored
307 glRectf( x,y,x+1,y+1 );
308 }
309 }
203687c working on energy monitoring
rtv authored
310
311 glPopMatrix();
312 }
427aa67 working on power models
rtv authored
313
314
315
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
316 void PowerPack::DissipationVis::Accumulate( meters_t x,
317 meters_t y,
318 joules_t amount )
427aa67 working on power models
rtv authored
319 {
165fe8b fixed energy dissipation vis bug
rtv authored
320 //printf( "accumulate %.2f %.2f %.2f\n", x, y, amount );
321
3ce2d0e applied patch from Jeff Donner, fixing some signed/unsigned comparisons
rtv authored
322 int ix = (x+width/2.0)/cellsize;
323 int iy = (y+height/2.0)/cellsize;
427aa67 working on power models
rtv authored
324
c1cde49 eliminated crash or assert fail in powerpack when moving outside the …
rtv authored
325 // don't accumulate if we're outside the grid
3ce2d0e applied patch from Jeff Donner, fixing some signed/unsigned comparisons
rtv authored
326 if( ix < 0 || ix >= int(columns) || iy < 0 || iy >= int(rows) )
165fe8b fixed energy dissipation vis bug
rtv authored
327 return;
602b385 energy dissipation vis now uses vector instead of array
rtv authored
328
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
329 joules_t& j = cells[ ix + (iy*columns) ];
602b385 energy dissipation vis now uses vector instead of array
rtv authored
330
331 j += amount;
332 if( j > peak_value )
aadcd1b vis stuff
rtv authored
333 {
602b385 energy dissipation vis now uses vector instead of array
rtv authored
334 peak_value = j;
335
336 if( peak_value > global_peak_value )
337 global_peak_value = peak_value;
aadcd1b vis stuff
rtv authored
338 }
427aa67 working on power models
rtv authored
339 }
Something went wrong with that request. Please try again.