#include <msp/time/units.h>
#include <msp/time/utils.h>
#include "control.h"
+#include "tracktype.h"
#include "trafficmanager.h"
#include "train.h"
+using namespace std;
using namespace Msp;
#include <iostream>
-using namespace std;
namespace Marklin {
target_speed(0),
status("Unplaced"),
travel_dist(0),
- real_speed(0)
+ travel_speed(0),
+ pure_speed(false),
+ speed_scale(0.02),
+ speed_scale_weight(0),
+ cur_track(0)
{
trfc_mgr.add_train(this);
void Train::set_speed(unsigned speed)
{
- unsigned old_speed=target_speed;
+ if(!target_speed && speed)
+ travel_speed=static_cast<int>(round(speed*speed_scale*87*3.6/5))*5;
target_speed=speed;
if(!target_speed)
for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
i->block->reserve(0);
rsv_blocks.clear();
- try_reserve=Time::TimeStamp();
- loco.set_speed(0);
- if(old_speed)
- set_status("Stopped");
- }
- else
- {
- unsigned n=reserve_more();
- if(n==0)
- {
- try_reserve=Time::now()+2*Time::sec;
- set_status("Blocked");
- }
- else if(n==1)
- {
- loco.set_speed(3);
- try_reserve=Time::now()+2*Time::sec;
- set_status("Slow");
- }
- else
- {
- loco.set_speed(speed);
- if(!old_speed)
- set_status("Traveling --- kmh");
- }
}
+
+ update_speed();
+ pure_speed=false;
}
void Train::place(Block *block, unsigned entry)
}
cur_blocks.push_back(BlockRef(block, entry));
+ set_position(block->get_endpoints()[entry]);
set_status("Stopped");
}
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)
+ update_speed();
+
+ if(cur_track)
{
- unsigned n=reserve_more();
- if(n>=2)
- {
- loco.set_speed(target_speed);
- set_status("Traveling --- kmh");
- try_reserve=Time::TimeStamp();
- }
- else if(n==1)
+ 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(3);
- set_status("Slow");
- try_reserve=t+2*Time::sec;
+ 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);
}
}
if(i!=rsv_blocks.begin())
{
- real_speed=static_cast<int>(round(travel_dist/((Time::now()-last_entry_time)/Time::sec)*87*3.6/5))*5;
- set_status(format("Traveling %3d kmh", real_speed));
+ 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);
- cout<<"Advancing: block "<<j->block<<" (sensor "<<j->block->get_sensor_id()<<") length "<<block_len<<'\n';
travel_dist+=block_len;
}
last_entry_time=Time::now();
+ pure_speed=true;
cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
- cout<<"Train "<<name<<" advanced, "<<cur_blocks.size()<<" cur_blocks, "<<rsv_blocks.size()<<" rsv_blocks\n";
}
+ 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)
- {
- unsigned n=reserve_more();
- if(n==0)
- {
- loco.set_speed(0);
- try_reserve=Time::now()+2*Time::sec;
- set_status("Blocked");
- }
- else if(n==1)
- {
- loco.set_speed(3);
- try_reserve=Time::now()+2*Time::sec;
- set_status("Slow");
- }
- }
+ update_speed();
}
else
{
- cout<<"Train "<<name<<" finding blocks to free\n";
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;
}
if(target_speed)
if(!last)
return 0;
- cout<<"Train "<<name<<" 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())
last=0;
}
- cout<<" "<<rsv_blocks.size()<<" rsv_blocks\n";
-
return nsens;
}
+void Train::update_speed()
+{
+ if(!target_speed)
+ {
+ loco.set_speed(0);
+ try_reserve=Time::TimeStamp();
+ set_status("Stopped");
+ }
+ else
+ {
+ unsigned n=reserve_more();
+ if(n==0)
+ {
+ loco.set_speed(0);
+ pure_speed=false;
+ try_reserve=Time::now()+2*Time::sec;
+ set_status("Blocked");
+ }
+ else if(n==1)
+ {
+ 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);
}
+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);
+}
+
} // namespace Marklin
projects / r2c2.git / blobdiff