]> git.tdb.fi Git - r2c2.git/blobdiff - source/libmarklin/train.cpp
Add networking library and a remote control program
[r2c2.git] / source / libmarklin / train.cpp
index 1644802008ae4b575c62d052f6a6e41ba94d306e..42d3e01e1f87b1675d9edd32fd7439533ced4af4 100644 (file)
@@ -1,47 +1,89 @@
+/* $Id$
+
+This file is part of the MSP Märklin suite
+Copyright © 2006-2009 Mikkosoft Productions, Mikko Rasa
+Distributed under the GPL
+*/
+
+#include <cmath>
+#include <msp/strings/formatter.h>
 #include <msp/time/units.h>
 #include <msp/time/utils.h>
 #include "control.h"
+#include "except.h"
+#include "tracktype.h"
 #include "trafficmanager.h"
 #include "train.h"
 
+using namespace std;
 using namespace Msp;
 
 #include <iostream>
-using namespace std;
 
 namespace Marklin {
 
 Train::Train(TrafficManager &tm, Locomotive &l):
        trfc_mgr(tm),
        loco(l),
-       target_speed(0)
+       target_speed(0),
+       status("Unplaced"),
+       travel_dist(0),
+       travel_speed(0),
+       pure_speed(false),
+       speed_scale(0.02),
+       speed_scale_weight(0),
+       cur_track(0)
 {
        trfc_mgr.add_train(this);
 
-       const map<unsigned, Sensor *> &sensors=trfc_mgr.get_control().get_sensors();
+       loco.signal_reverse_changed.connect(sigc::mem_fun(this, &Train::locomotive_reverse_changed));
+
+       const map<unsigned, Sensor *> &sensors = trfc_mgr.get_control().get_sensors();
        for(map<unsigned, Sensor *>::const_iterator i=sensors.begin(); i!=sensors.end(); ++i)
                i->second->signal_state_changed.connect(sigc::bind(sigc::mem_fun(this, &Train::sensor_event), i->second));
+
+       const map<unsigned, Turnout *> &turnouts = trfc_mgr.get_control().get_turnouts();
+       for(map<unsigned, Turnout *>::const_iterator i=turnouts.begin(); i!=turnouts.end(); ++i)
+       {
+               i->second->signal_route_changing.connect(sigc::bind(sigc::mem_fun(this, &Train::turnout_route_changing), i->second));
+               i->second->signal_route_changed.connect(sigc::bind(sigc::mem_fun(this, &Train::turnout_route_changed), i->second));
+       }
 }
 
 void Train::set_name(const string &n)
 {
-       name=n;
+       name = n;
 
        signal_name_changed.emit(name);
 }
 
 void Train::set_speed(unsigned speed)
 {
-       target_speed=speed;
+       if(speed==target_speed)
+               return;
+       if(!target_speed && speed)
+               travel_speed = static_cast<int>(round(speed*speed_scale*87*3.6/5))*5;
+
+       target_speed = speed;
        if(!target_speed)
        {
+               // XXX We might roll onto a new sensor and get confused - should delay freeing blocks a bit
                for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
                        i->block->reserve(0);
                rsv_blocks.clear();
        }
-       else if(rsv_blocks.empty() && !reserve_more())
-               return;
-       loco.set_speed(speed);
+       else
+               reserve_more();
+
+       signal_target_speed_changed.emit(target_speed);
+
+       update_speed();
+       pure_speed = false;
+}
+
+void Train::set_reverse(bool rev)
+{
+       loco.set_reverse(rev);
 }
 
 void Train::place(Block *block, unsigned entry)
@@ -49,57 +91,107 @@ void Train::place(Block *block, unsigned entry)
        for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end();)
        {
                i->block->reserve(0);
-               i=rsv_blocks.erase(i);
+               i = rsv_blocks.erase(i);
        }
 
        for(list<BlockRef>::iterator i=cur_blocks.begin(); i!=cur_blocks.end();)
        {
                i->block->reserve(0);
-               i=cur_blocks.erase(i);
+               i = cur_blocks.erase(i);
        }
 
        if(!block->reserve(this))
+       {
+               set_status("Unplaced");
                return;
+       }
 
        cur_blocks.push_back(BlockRef(block, entry));
+       set_position(block->get_endpoints()[entry]);
+
+       set_status("Stopped");
 }
 
 bool Train::free_block(Block *block)
 {
+       unsigned nsens = 0;
        for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
+       {
                if(i->block==block)
                {
+                       if(nsens<1)
+                               return false;
                        while(i!=rsv_blocks.end())
                        {
                                i->block->reserve(0);
-                               i=rsv_blocks.erase(i);
+                               i = rsv_blocks.erase(i);
                        }
+                       update_speed();
                        return true;
                }
+               else if(i->block->get_sensor_id())
+                       ++nsens;
+       }
 
        return false;
 }
 
-void Train::tick(const Time::TimeStamp &t)
+void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 {
        if(try_reserve && t>try_reserve)
        {
-               if(reserve_more())
+               reserve_more();
+               update_speed();
+       }
+
+       if(cur_track)
+       {
+               unsigned route = 0;
+               if(cur_track->get_turnout_id())
+                       route = trfc_mgr.get_control().get_turnout(cur_track->get_turnout_id()).get_route();
+
+               offset += speed_scale*loco.get_speed()*(dt/Time::sec);
+               if(offset>cur_track->get_type().get_route_length(route))
                {
-                       loco.set_speed(target_speed);
-                       try_reserve=Time::TimeStamp();
+                       int out = cur_track->traverse(cur_track_ep, route);
+                       if(out>=0)
+                       {
+                               Track *next = cur_track->get_link(out);
+                               if(next)
+                                       cur_track_ep = next->get_endpoint_by_link(*cur_track);
+                               cur_track = next;
+                               offset = 0;
+                       }
+                       else
+                               cur_track = 0;
                }
-               else
-                       try_reserve=t+2*Time::sec;
+
+               if(cur_track)
+                       pos = cur_track->get_point(cur_track_ep, route, offset);
        }
 }
 
-void Train::sensor_event(bool state, Sensor *sensor)
+void Train::save(list<DataFile::Statement> &st) const
 {
-       if(!loco.get_speed())
-               return;
+       st.push_back((DataFile::Statement("name"), name));
+       st.push_back((DataFile::Statement("speed_scale"), speed_scale, speed_scale_weight));
+}
 
-       unsigned addr=sensor->get_address();
+void Train::locomotive_reverse_changed(bool)
+{
+       for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
+               i->block->reserve(0);
+       rsv_blocks.clear();
+       cur_blocks.reverse();
+       for(list<BlockRef>::iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
+               i->entry = i->block->traverse(i->entry);
+       reserve_more();
+       update_speed();
+}
+
+void Train::sensor_event(bool state, Sensor *sensor)
+{
+       unsigned addr = sensor->get_address();
 
        if(state)
        {
@@ -107,63 +199,135 @@ void Train::sensor_event(bool state, Sensor *sensor)
                for(i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
                        if(i->block->get_sensor_id() && i->block->get_sensor_id()!=addr)
                                break;
-               cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
 
-               cout<<"Train "<<this<<" advanced, "<<cur_blocks.size()<<" cur_blocks, "<<rsv_blocks.size()<<" rsv_blocks\n";
+               if(i!=rsv_blocks.begin())
+               {
+                       float travel_time_secs = (Time::now()-last_entry_time)/Time::sec;
+                       travel_speed = static_cast<int>(round(travel_dist/travel_time_secs*87*3.6/5))*5;
+
+                       if(pure_speed)
+                       {
+                               float weight = loco.get_speed()*travel_dist;
+                               if(weight)
+                               {
+                                       weight *= weight;
+                                       float scale = travel_dist/travel_time_secs/loco.get_speed();
+                                       cout<<"Updating speed_scale: "<<speed_scale<<'x'<<speed_scale_weight<<" + "<<scale<<'x'<<weight<<'\n';
+                                       speed_scale = (speed_scale*speed_scale_weight+scale*weight)/(speed_scale_weight+weight);
+                                       speed_scale_weight += weight;
+                                       cout<<"  Result: "<<speed_scale<<'x'<<speed_scale_weight<<'\n';
+                               }
+                       }
+
+                       travel_dist = 0;
+                       float block_len;
+                       for(list<BlockRef>::iterator j=rsv_blocks.begin(); j!=i; ++j)
+                       {
+                               j->block->traverse(j->entry, &block_len);
+                               travel_dist += block_len;
+                       }
+                       last_entry_time = Time::now();
+                       pure_speed = true;
+
+                       cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
+               }
 
-               if(rsv_blocks.empty() && !reserve_more())
+               for(i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
+                       if(i->block->get_sensor_id()==addr)
+                               set_position(i->block->get_endpoints()[i->entry]);
+
+               if(target_speed)
                {
-                       loco.set_speed(0);
-                       try_reserve=Time::now()+2*Time::sec;
+                       reserve_more();
+                       update_speed();
                }
        }
        else
        {
-               cout<<"Train "<<this<<" finding blocks to free\n";
-               list<BlockRef>::iterator i;
-               for(i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
-               {
+               for(list<BlockRef>::iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
                        if(i->block->get_sensor_id()==addr)
                        {
                                ++i;
                                for(list<BlockRef>::iterator j=cur_blocks.begin(); j!=i; ++j)
                                        j->block->reserve(0);
-                               cout<<"  "<<distance(cur_blocks.begin(), i)<<" blocks freed, ";
                                cur_blocks.erase(cur_blocks.begin(), i);
-                               cout<<cur_blocks.size()<<" cur_blocks\n";
+                               break;
                        }
-               }
 
-               reserve_more();
+               if(target_speed)
+                       reserve_more();
+       }
+}
+
+void Train::turnout_route_changing(unsigned, Turnout *turnout)
+{
+       unsigned tid = turnout->get_address();
+       for(list<BlockRef>::const_iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
+               if(i->block->get_turnout_id()==tid)
+                       throw TurnoutBusy(this);
+       
+       unsigned nsens = 0;
+       for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
+       {
+               if(i->block->get_turnout_id()==tid)
+               {
+                       if(nsens<1)
+                               throw TurnoutBusy(this);
+                       break;
+               }
+               else if(i->block->get_sensor_id())
+                       ++nsens;
        }
 }
 
-bool Train::reserve_more()
+void Train::turnout_route_changed(unsigned, Turnout *turnout)
 {
-       BlockRef *last=0;
+       unsigned tid = turnout->get_address();
+       for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
+               if(i->block->get_turnout_id()==tid)
+               {
+                       while(i!=rsv_blocks.end())
+                       {
+                               i->block->reserve(0);
+                               i = rsv_blocks.erase(i);
+                       }
+                       reserve_more();
+                       update_speed();
+                       return;
+               }
+}
+
+unsigned Train::reserve_more()
+{
+       BlockRef *last = 0;
        if(!rsv_blocks.empty())
-               last=&rsv_blocks.back();
+               last = &rsv_blocks.back();
        else if(!cur_blocks.empty())
-               last=&cur_blocks.back();
+               last = &cur_blocks.back();
        if(!last)
-               return false;
+               return 0;
 
-       cout<<"Train "<<this<<" reserving more blocks\n";
+       unsigned nsens = 0;
+       for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
+               if(i->block->get_sensor_id())
+                       ++nsens;
 
-       bool result=false;
-       unsigned size=rsv_blocks.size();
-       while(size<3)
+       bool result = false;
+       while(nsens<2)
        {
-               int exit=last->block->traverse(last->entry);
+               int exit = last->block->traverse(last->entry);
                if(exit>=0) 
                {
-                       Block *link=last->block->get_link(exit);
+                       Block *link = last->block->get_link(exit);
                        if(link && link->reserve(this))
                        {
                                rsv_blocks.push_back(BlockRef(link, link->get_endpoint_by_link(*last->block)));
-                               last=&rsv_blocks.back();
-                               ++size;
-                               result=true;
+                               last = &rsv_blocks.back();
+                               if(last->block->get_sensor_id())
+                               {
+                                       ++nsens;
+                                       result = true;
+                               }
                        }
                        else
                                break;
@@ -172,9 +336,77 @@ bool Train::reserve_more()
                        break;
        }
 
-       cout<<"  "<<rsv_blocks.size()<<" rsv_blocks\n";
+       while(last && !last->block->get_sensor_id())
+       {
+               last->block->reserve(0);
+               rsv_blocks.erase(--rsv_blocks.end());
+               if(!rsv_blocks.empty())
+                       last = &rsv_blocks.back();
+               else
+                       last = 0;
+       }
+
+       return nsens;
+}
+
+void Train::update_speed()
+{
+       if(!target_speed)
+       {
+               loco.set_speed(0);
+               try_reserve = Time::TimeStamp();
+               set_status("Stopped");
+       }
+       else
+       {
+               unsigned nsens = 0;
+               for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
+                       if(i->block->get_sensor_id())
+                               ++nsens;
+
+               if(nsens==0)
+               {
+                       loco.set_speed(0);
+                       pure_speed = false;
+                       try_reserve = Time::now()+2*Time::sec;
+                       set_status("Blocked");
+               }
+               else if(nsens==1 && target_speed>3)
+               {
+                       loco.set_speed(3);
+                       pure_speed = false;
+                       try_reserve = Time::now()+2*Time::sec;
+                       set_status("Slow");
+               }
+               else
+               {
+                       loco.set_speed(target_speed);
+                       try_reserve = Time::TimeStamp();
+                       set_status(format("Traveling %d kmh", travel_speed));
+               }
+       }
+}
+
+void Train::set_status(const string &s)
+{
+       status = s;
+       signal_status_changed.emit(s);
+}
 
-       return result;
+void Train::set_position(const Block::Endpoint &bep)
+{
+       cur_track = bep.track;
+       cur_track_ep = bep.track_ep;
+       offset = 0;
+       pos = cur_track->get_endpoint_position(cur_track_ep);
+}
+
+
+Train::Loader::Loader(Train &t):
+       DataFile::BasicLoader<Train>(t)
+{
+       add("name",        &Train::name);
+       add("speed_scale", &Train::speed_scale, &Train::speed_scale_weight);
 }
 
 } // namespace Marklin