source/libmarklin/train.cpp

   1 /* $Id$
   2
   3 This file is part of the MSP Märklin suite
   4 Copyright © 2006-2009 Mikkosoft Productions, Mikko Rasa
   5 Distributed under the GPL
   6 */
   7
   8 #include <cmath>
   9 #include <msp/strings/formatter.h>
  10 #include <msp/time/units.h>
  11 #include <msp/time/utils.h>
  12 #include "control.h"
  13 #include "tracktype.h"
  14 #include "trafficmanager.h"
  15 #include "train.h"
  16
  17 using namespace std;
  18 using namespace Msp;
  19
  20 #include <iostream>
  21
  22 namespace Marklin {
  23
  24 Train::Train(TrafficManager &tm, Locomotive &l):
  25         trfc_mgr(tm),
  26         loco(l),
  27         target_speed(0),
  28         status("Unplaced"),
  29         travel_dist(0),
  30         travel_speed(0),
  31         pure_speed(false),
  32         speed_scale(0.02),
  33         speed_scale_weight(0),
  34         cur_track(0)
  35 {
  36         trfc_mgr.add_train(this);
  37
  38         const map<unsigned, Sensor *> &sensors=trfc_mgr.get_control().get_sensors();
  39         for(map<unsigned, Sensor *>::const_iterator i=sensors.begin(); i!=sensors.end(); ++i)
  40                 i->second->signal_state_changed.connect(sigc::bind(sigc::mem_fun(this, &Train::sensor_event), i->second));
  41 }
  42
  43 void Train::set_name(const string &n)
  44 {
  45         name=n;
  46
  47         signal_name_changed.emit(name);
  48 }
  49
  50 void Train::set_speed(unsigned speed)
  51 {
  52         if(!target_speed && speed)
  53                 travel_speed=static_cast<int>(round(speed*speed_scale*87*3.6/5))*5;
  54
  55         target_speed=speed;
  56         if(!target_speed)
  57         {
  58                 // XXX We might roll onto a new sensor and get confused - should delay freeing blocks a bit
  59                 for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
  60                         i->block->reserve(0);
  61                 rsv_blocks.clear();
  62         }
  63
  64         update_speed();
  65         pure_speed=false;
  66 }
  67
  68 void Train::place(Block *block, unsigned entry)
  69 {
  70         for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end();)
  71         {
  72                 i->block->reserve(0);
  73                 i=rsv_blocks.erase(i);
  74         }
  75
  76         for(list<BlockRef>::iterator i=cur_blocks.begin(); i!=cur_blocks.end();)
  77         {
  78                 i->block->reserve(0);
  79                 i=cur_blocks.erase(i);
  80         }
  81
  82         if(!block->reserve(this))
  83         {
  84                 set_status("Unplaced");
  85                 return;
  86         }
  87
  88         cur_blocks.push_back(BlockRef(block, entry));
  89         set_position(block->get_endpoints()[entry]);
  90
  91         set_status("Stopped");
  92 }
  93
  94 bool Train::free_block(Block *block)
  95 {
  96         for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
  97                 if(i->block==block)
  98                 {
  99                         while(i!=rsv_blocks.end())
 100                         {
 101                                 i->block->reserve(0);
 102                                 i=rsv_blocks.erase(i);
 103                         }
 104                         return true;
 105                 }
 106
 107         return false;
 108 }
 109
 110 void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 111 {
 112         if(try_reserve && t>try_reserve)
 113                 update_speed();
 114
 115         if(cur_track)
 116         {
 117                 unsigned route=0;
 118                 if(cur_track->get_turnout_id())
 119                         route=trfc_mgr.get_control().get_turnout(cur_track->get_turnout_id()).get_route();
 120
 121                 offset+=speed_scale*loco.get_speed()*(dt/Time::sec);
 122                 if(offset>cur_track->get_type().get_route_length(route))
 123                 {
 124                         int out=cur_track->traverse(cur_track_ep, route);
 125                         if(out>=0)
 126                         {
 127                                 Track *next=cur_track->get_link(out);
 128                                 if(next)
 129                                         cur_track_ep=next->get_endpoint_by_link(*cur_track);
 130                                 cur_track=next;
 131                                 offset=0;
 132                         }
 133                         else
 134                                 cur_track=0;
 135                 }
 136
 137                 if(cur_track)
 138                         pos=cur_track->get_point(cur_track_ep, route, offset);
 139         }
 140 }
 141
 142 void Train::save(list<DataFile::Statement> &st) const
 143 {
 144         st.push_back((DataFile::Statement("name"), name));
 145         st.push_back((DataFile::Statement("speed_scale"), speed_scale, speed_scale_weight));
 146 }
 147
 148 void Train::sensor_event(bool state, Sensor *sensor)
 149 {
 150         unsigned addr=sensor->get_address();
 151
 152         if(state)
 153         {
 154                 list<BlockRef>::iterator i;
 155                 for(i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 156                         if(i->block->get_sensor_id() && i->block->get_sensor_id()!=addr)
 157                                 break;
 158
 159                 if(i!=rsv_blocks.begin())
 160                 {
 161                         float travel_time_secs=(Time::now()-last_entry_time)/Time::sec;
 162                         travel_speed=static_cast<int>(round(travel_dist/travel_time_secs*87*3.6/5))*5;
 163
 164                         if(pure_speed)
 165                         {
 166                                 float weight=loco.get_speed()*travel_dist;
 167                                 if(weight)
 168                                 {
 169                                         weight*=weight;
 170                                         float scale=travel_dist/travel_time_secs/loco.get_speed();
 171                                         cout<<"Updating speed_scale: "<<speed_scale<<'x'<<speed_scale_weight<<" + "<<scale<<'x'<<weight<<'\n';
 172                                         speed_scale=(speed_scale*speed_scale_weight+scale*weight)/(speed_scale_weight+weight);
 173                                         speed_scale_weight+=weight;
 174                                         cout<<"  Result: "<<speed_scale<<'x'<<speed_scale_weight<<'\n';
 175                                 }
 176                         }
 177
 178                         travel_dist=0;
 179                         float block_len;
 180                         for(list<BlockRef>::iterator j=rsv_blocks.begin(); j!=i; ++j)
 181                         {
 182                                 j->block->traverse(j->entry, &block_len);
 183                                 travel_dist+=block_len;
 184                         }
 185                         last_entry_time=Time::now();
 186                         pure_speed=true;
 187
 188                         cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
 189                 }
 190
 191                 for(i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
 192                         if(i->block->get_sensor_id()==addr)
 193                                 set_position(i->block->get_endpoints()[i->entry]);
 194
 195                 if(target_speed)
 196                         update_speed();
 197         }
 198         else
 199         {
 200                 for(list<BlockRef>::iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
 201                         if(i->block->get_sensor_id()==addr)
 202                         {
 203                                 ++i;
 204                                 for(list<BlockRef>::iterator j=cur_blocks.begin(); j!=i; ++j)
 205                                         j->block->reserve(0);
 206                                 cur_blocks.erase(cur_blocks.begin(), i);
 207                                 break;
 208                         }
 209
 210                 if(target_speed)
 211                         reserve_more();
 212         }
 213 }
 214
 215 unsigned Train::reserve_more()
 216 {
 217         BlockRef *last=0;
 218         if(!rsv_blocks.empty())
 219                 last=&rsv_blocks.back();
 220         else if(!cur_blocks.empty())
 221                 last=&cur_blocks.back();
 222         if(!last)
 223                 return 0;
 224
 225         unsigned nsens=0;
 226         for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 227                 if(i->block->get_sensor_id())
 228                         ++nsens;
 229
 230         bool result=false;
 231         while(nsens<2)
 232         {
 233                 int exit=last->block->traverse(last->entry);
 234                 if(exit>=0)
 235                 {
 236                         Block *link=last->block->get_link(exit);
 237                         if(link && link->reserve(this))
 238                         {
 239                                 rsv_blocks.push_back(BlockRef(link, link->get_endpoint_by_link(*last->block)));
 240                                 last=&rsv_blocks.back();
 241                                 if(last->block->get_sensor_id())
 242                                 {
 243                                         ++nsens;
 244                                         result=true;
 245                                 }
 246                         }
 247                         else
 248                                 break;
 249                 }
 250                 else
 251                         break;
 252         }
 253
 254         while(last && !last->block->get_sensor_id())
 255         {
 256                 last->block->reserve(0);
 257                 rsv_blocks.erase(--rsv_blocks.end());
 258                 if(!rsv_blocks.empty())
 259                         last=&rsv_blocks.back();
 260                 else
 261                         last=0;
 262         }
 263
 264         return nsens;
 265 }
 266
 267 void Train::update_speed()
 268 {
 269         if(!target_speed)
 270         {
 271                 loco.set_speed(0);
 272                 try_reserve=Time::TimeStamp();
 273                 set_status("Stopped");
 274         }
 275         else
 276         {
 277                 unsigned n=reserve_more();
 278                 if(n==0)
 279                 {
 280                         loco.set_speed(0);
 281                         pure_speed=false;
 282                         try_reserve=Time::now()+2*Time::sec;
 283                         set_status("Blocked");
 284                 }
 285                 else if(n==1)
 286                 {
 287                         loco.set_speed(3);
 288                         pure_speed=false;
 289                         try_reserve=Time::now()+2*Time::sec;
 290                         set_status("Slow");
 291                 }
 292                 else
 293                 {
 294                         loco.set_speed(target_speed);
 295                         try_reserve=Time::TimeStamp();
 296                         set_status(format("Traveling %d kmh", travel_speed));
 297                 }
 298         }
 299 }
 300
 301 void Train::set_status(const string &s)
 302 {
 303         status=s;
 304         signal_status_changed.emit(s);
 305 }
 306
 307 void Train::set_position(const Block::Endpoint &bep)
 308 {
 309         cur_track=bep.track;
 310         cur_track_ep=bep.track_ep;
 311         offset=0;
 312         pos=cur_track->get_endpoint_position(cur_track_ep);
 313 }
 314
 315
 316 Train::Loader::Loader(Train &t):
 317         DataFile::BasicLoader<Train>(t)
 318 {
 319         add("name",        &Train::name);
 320         add("speed_scale", &Train::speed_scale, &Train::speed_scale_weight);
 321 }
 322
 323 } // namespace Marklin