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::sensor_event(bool state, Sensor *sensor)
 143 {
 144         unsigned addr=sensor->get_address();
 145
 146         if(state)
 147         {
 148                 list<BlockRef>::iterator i;
 149                 for(i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 150                         if(i->block->get_sensor_id() && i->block->get_sensor_id()!=addr)
 151                                 break;
 152
 153                 if(i!=rsv_blocks.begin())
 154                 {
 155                         float travel_time_secs=(Time::now()-last_entry_time)/Time::sec;
 156                         travel_speed=static_cast<int>(round(travel_dist/travel_time_secs*87*3.6/5))*5;
 157
 158                         if(pure_speed)
 159                         {
 160                                 float weight=loco.get_speed()*travel_dist;
 161                                 if(weight)
 162                                 {
 163                                         weight*=weight;
 164                                         float scale=travel_dist/travel_time_secs/loco.get_speed();
 165                                         cout<<"Updating speed_scale: "<<speed_scale<<'x'<<speed_scale_weight<<" + "<<scale<<'x'<<weight<<'\n';
 166                                         speed_scale=(speed_scale*speed_scale_weight+scale*weight)/(speed_scale_weight+weight);
 167                                         speed_scale_weight+=weight;
 168                                         cout<<"  Result: "<<speed_scale<<'x'<<speed_scale_weight<<'\n';
 169                                 }
 170                         }
 171
 172                         travel_dist=0;
 173                         float block_len;
 174                         for(list<BlockRef>::iterator j=rsv_blocks.begin(); j!=i; ++j)
 175                         {
 176                                 j->block->traverse(j->entry, &block_len);
 177                                 travel_dist+=block_len;
 178                         }
 179                         last_entry_time=Time::now();
 180                         pure_speed=true;
 181
 182                         cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
 183                 }
 184
 185                 for(i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
 186                         if(i->block->get_sensor_id()==addr)
 187                                 set_position(i->block->get_endpoints()[i->entry]);
 188
 189                 if(target_speed)
 190                         update_speed();
 191         }
 192         else
 193         {
 194                 for(list<BlockRef>::iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
 195                         if(i->block->get_sensor_id()==addr)
 196                         {
 197                                 ++i;
 198                                 for(list<BlockRef>::iterator j=cur_blocks.begin(); j!=i; ++j)
 199                                         j->block->reserve(0);
 200                                 cur_blocks.erase(cur_blocks.begin(), i);
 201                                 break;
 202                         }
 203
 204                 if(target_speed)
 205                         reserve_more();
 206         }
 207 }
 208
 209 unsigned Train::reserve_more()
 210 {
 211         BlockRef *last=0;
 212         if(!rsv_blocks.empty())
 213                 last=&rsv_blocks.back();
 214         else if(!cur_blocks.empty())
 215                 last=&cur_blocks.back();
 216         if(!last)
 217                 return 0;
 218
 219         unsigned nsens=0;
 220         for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 221                 if(i->block->get_sensor_id())
 222                         ++nsens;
 223
 224         bool result=false;
 225         while(nsens<2)
 226         {
 227                 int exit=last->block->traverse(last->entry);
 228                 if(exit>=0)
 229                 {
 230                         Block *link=last->block->get_link(exit);
 231                         if(link && link->reserve(this))
 232                         {
 233                                 rsv_blocks.push_back(BlockRef(link, link->get_endpoint_by_link(*last->block)));
 234                                 last=&rsv_blocks.back();
 235                                 if(last->block->get_sensor_id())
 236                                 {
 237                                         ++nsens;
 238                                         result=true;
 239                                 }
 240                         }
 241                         else
 242                                 break;
 243                 }
 244                 else
 245                         break;
 246         }
 247
 248         while(last && !last->block->get_sensor_id())
 249         {
 250                 last->block->reserve(0);
 251                 rsv_blocks.erase(--rsv_blocks.end());
 252                 if(!rsv_blocks.empty())
 253                         last=&rsv_blocks.back();
 254                 else
 255                         last=0;
 256         }
 257
 258         return nsens;
 259 }
 260
 261 void Train::update_speed()
 262 {
 263         if(!target_speed)
 264         {
 265                 loco.set_speed(0);
 266                 try_reserve=Time::TimeStamp();
 267                 set_status("Stopped");
 268         }
 269         else
 270         {
 271                 unsigned n=reserve_more();
 272                 if(n==0)
 273                 {
 274                         loco.set_speed(0);
 275                         pure_speed=false;
 276                         try_reserve=Time::now()+2*Time::sec;
 277                         set_status("Blocked");
 278                 }
 279                 else if(n==1)
 280                 {
 281                         loco.set_speed(3);
 282                         pure_speed=false;
 283                         try_reserve=Time::now()+2*Time::sec;
 284                         set_status("Slow");
 285                 }
 286                 else
 287                 {
 288                         loco.set_speed(target_speed);
 289                         try_reserve=Time::TimeStamp();
 290                         set_status(format("Traveling %d kmh", travel_speed));
 291                 }
 292         }
 293 }
 294
 295 void Train::set_status(const string &s)
 296 {
 297         status=s;
 298         signal_status_changed.emit(s);
 299 }
 300
 301 void Train::set_position(const Block::Endpoint &bep)
 302 {
 303         cur_track=bep.track;
 304         cur_track_ep=bep.track_ep;
 305         offset=0;
 306         pos=cur_track->get_endpoint_position(cur_track_ep);
 307 }
 308
 309 } // namespace Marklin