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 "trafficmanager.h"
  14 #include "train.h"
  15
  16 using namespace Msp;
  17
  18 #include <iostream>
  19 using namespace std;
  20
  21 namespace Marklin {
  22
  23 Train::Train(TrafficManager &tm, Locomotive &l):
  24         trfc_mgr(tm),
  25         loco(l),
  26         target_speed(0),
  27         status("Unplaced"),
  28         travel_dist(0),
  29         real_speed(0)
  30 {
  31         trfc_mgr.add_train(this);
  32
  33         const map<unsigned, Sensor *> &sensors=trfc_mgr.get_control().get_sensors();
  34         for(map<unsigned, Sensor *>::const_iterator i=sensors.begin(); i!=sensors.end(); ++i)
  35                 i->second->signal_state_changed.connect(sigc::bind(sigc::mem_fun(this, &Train::sensor_event), i->second));
  36 }
  37
  38 void Train::set_name(const string &n)
  39 {
  40         name=n;
  41
  42         signal_name_changed.emit(name);
  43 }
  44
  45 void Train::set_speed(unsigned speed)
  46 {
  47         unsigned old_speed=target_speed;
  48
  49         target_speed=speed;
  50         if(!target_speed)
  51         {
  52                 // XXX We might roll onto a new sensor and get confused - should delay freeing blocks a bit
  53                 for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
  54                         i->block->reserve(0);
  55                 rsv_blocks.clear();
  56                 try_reserve=Time::TimeStamp();
  57                 loco.set_speed(0);
  58                 if(old_speed)
  59                         set_status("Stopped");
  60         }
  61         else
  62         {
  63                 unsigned n=reserve_more();
  64                 if(n==0)
  65                 {
  66                         try_reserve=Time::now()+2*Time::sec;
  67                         set_status("Blocked");
  68                 }
  69                 else if(n==1)
  70                 {
  71                         loco.set_speed(3);
  72                         try_reserve=Time::now()+2*Time::sec;
  73                         set_status("Slow");
  74                 }
  75                 else
  76                 {
  77                         loco.set_speed(speed);
  78                         if(!old_speed)
  79                                 set_status("Traveling --- kmh");
  80                 }
  81         }
  82 }
  83
  84 void Train::place(Block *block, unsigned entry)
  85 {
  86         for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end();)
  87         {
  88                 i->block->reserve(0);
  89                 i=rsv_blocks.erase(i);
  90         }
  91
  92         for(list<BlockRef>::iterator i=cur_blocks.begin(); i!=cur_blocks.end();)
  93         {
  94                 i->block->reserve(0);
  95                 i=cur_blocks.erase(i);
  96         }
  97
  98         if(!block->reserve(this))
  99         {
 100                 set_status("Unplaced");
 101                 return;
 102         }
 103
 104         cur_blocks.push_back(BlockRef(block, entry));
 105
 106         set_status("Stopped");
 107 }
 108
 109 bool Train::free_block(Block *block)
 110 {
 111         for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 112                 if(i->block==block)
 113                 {
 114                         while(i!=rsv_blocks.end())
 115                         {
 116                                 i->block->reserve(0);
 117                                 i=rsv_blocks.erase(i);
 118                         }
 119                         return true;
 120                 }
 121
 122         return false;
 123 }
 124
 125 void Train::tick(const Time::TimeStamp &t)
 126 {
 127         if(try_reserve && t>try_reserve)
 128         {
 129                 unsigned n=reserve_more();
 130                 if(n>=2)
 131                 {
 132                         loco.set_speed(target_speed);
 133                         set_status("Traveling --- kmh");
 134                         try_reserve=Time::TimeStamp();
 135                 }
 136                 else if(n==1)
 137                 {
 138                         loco.set_speed(3);
 139                         set_status("Slow");
 140                         try_reserve=t+2*Time::sec;
 141                 }
 142                 else
 143                         try_reserve=t+2*Time::sec;
 144         }
 145 }
 146
 147 void Train::sensor_event(bool state, Sensor *sensor)
 148 {
 149         unsigned addr=sensor->get_address();
 150
 151         if(state)
 152         {
 153                 list<BlockRef>::iterator i;
 154                 for(i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 155                         if(i->block->get_sensor_id() && i->block->get_sensor_id()!=addr)
 156                                 break;
 157
 158                 if(i!=rsv_blocks.begin())
 159                 {
 160                         real_speed=static_cast<int>(round(travel_dist/((Time::now()-last_entry_time)/Time::sec)*87*3.6/5))*5;
 161                         set_status(format("Traveling %3d kmh", real_speed));
 162
 163                         travel_dist=0;
 164                         float block_len;
 165                         for(list<BlockRef>::iterator j=rsv_blocks.begin(); j!=i; ++j)
 166                         {
 167                                 j->block->traverse(j->entry, &block_len);
 168                                 cout<<"Advancing: block "<<j->block<<" (sensor "<<j->block->get_sensor_id()<<") length "<<block_len<<'\n';
 169                                 travel_dist+=block_len;
 170                         }
 171                         last_entry_time=Time::now();
 172
 173                         cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
 174                         cout<<"Train "<<name<<" advanced, "<<cur_blocks.size()<<" cur_blocks, "<<rsv_blocks.size()<<" rsv_blocks\n";
 175                 }
 176
 177                 if(target_speed)
 178                 {
 179                         unsigned n=reserve_more();
 180                         if(n==0)
 181                         {
 182                                 loco.set_speed(0);
 183                                 try_reserve=Time::now()+2*Time::sec;
 184                                 set_status("Blocked");
 185                         }
 186                         else if(n==1)
 187                         {
 188                                 loco.set_speed(3);
 189                                 try_reserve=Time::now()+2*Time::sec;
 190                                 set_status("Slow");
 191                         }
 192                 }
 193         }
 194         else
 195         {
 196                 cout<<"Train "<<name<<" finding blocks to free\n";
 197                 for(list<BlockRef>::iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
 198                         if(i->block->get_sensor_id()==addr)
 199                         {
 200                                 ++i;
 201                                 for(list<BlockRef>::iterator j=cur_blocks.begin(); j!=i; ++j)
 202                                         j->block->reserve(0);
 203                                 cout<<"  "<<distance(cur_blocks.begin(), i)<<" blocks freed, ";
 204                                 cur_blocks.erase(cur_blocks.begin(), i);
 205                                 cout<<cur_blocks.size()<<" cur_blocks\n";
 206                         }
 207
 208                 if(target_speed)
 209                         reserve_more();
 210         }
 211 }
 212
 213 unsigned Train::reserve_more()
 214 {
 215         BlockRef *last=0;
 216         if(!rsv_blocks.empty())
 217                 last=&rsv_blocks.back();
 218         else if(!cur_blocks.empty())
 219                 last=&cur_blocks.back();
 220         if(!last)
 221                 return 0;
 222
 223         cout<<"Train "<<name<<" reserving more blocks\n";
 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         cout<<"  "<<rsv_blocks.size()<<" rsv_blocks\n";
 265
 266         return nsens;
 267 }
 268
 269 void Train::set_status(const string &s)
 270 {
 271         status=s;
 272         signal_status_changed.emit(s);
 273 }
 274
 275 } // namespace Marklin