Search  

---

C:/temp/src/j2k/QNX4/Group5/Model.cpp

Go to the documentation of this file.

/***
   NAME
     model
   PURPOSE
     model for fake train/track until the lab's ready
   HISTORY
     b_goudre -- Feb 28, 2000: Created.
***/

#ifndef __J2K__Train__Model_CPP__
#define __J2K__Train__Model_CPP__

#include <math.h>
#include "model.hh"

// track, direction, switch
const char Model_track::next_sector_[][][] = 
{
    { { 2, -1 }, { 16,-1 } },   // 1
    { { 3, 5  }, { 1, -1 } },   // 2
    { { 4, -1 }, { 2, -1 } },   // 3
    { { 7, -1 }, { 3, -1 } },   // 4
    { { 6, -1 }, { 2, -1 } },   // 5
    { { 7, -1 }, { 5, -1 } },   // 6
    { { 8, -1 }, { 4, 6  } },   // 7
    { { 9, -1 }, { 7, -1 } },   // 8
    { { 10,-1 }, { 8, -1 } },   // 9
    { { 13, 11}, { 9, -1 } },   // 10
    { { 12, -1}, {10, -1 } },   // 11
    { { 15, -1}, {11, -1 } },   // 12
    { { 14, -1}, {10, -1 } },   // 13
    { { 15, -1}, {13, -1 } },   // 14
    { { 16, -1}, {14, 12 } },   // 15
    { { 1,  -1}, {15, -1 } }    // 16
};

const char Model_track::relevant_switch_[] = 
{
  -1,  // 1
   3,  // 2
  -1,  // 3
  -1,  // 4
  -1,  // 5
  -1,  // 6
   2,  // 7
  -1,  // 8
  -1,  // 9
   1,  // 10
  -1,  // 11
  -1,  // 12
  -1,  // 13
  -1,  // 14
   0,  // 15
  -1   // 16
};

static const double Speed_scale = 1.0 / ( 14.0 * 4 );

void Model_track::position( double  speed,
                            double& pos,
                            char&   sector)
{
  char s;
  int sign;
  pos += speed * Speed_scale;
  if (pos < 0.0) {          // counter
    pos += 1.0;
    if ( relevant_switch_[sector-1] != -1 ) {
      sector = next_sector_[sector-1][1][relevant_switch_[sector-1]];
    } else {
      sector = next_sector_[sector-1][1][0];
    }
  } else if( pos > 1.0 ) {
    pos -= 1.0;
    if ( relevant_switch_[sector-1] != -1 ) {
      sector = next_sector_[sector-1][0][relevant_switch_[sector-1]];
    } else {
      sector = next_sector_[sector-1][0][0];
    }
  }

  // We just hope we don't cross more than 1 sector at a time...
  // this is a matter of fixing the scale factor properly.
}

double Model_train::friction_ = 0.25;

Model_train::Model_train(Model_track* t, char sect, double inert, double pos)
  : track_(t),
    sector_(sect),
    inertia_(inert),
    pos_in_track_(pos),
    speed_(0.0),
    actual_speed_(0.0)
{ }

void Model_train::tick()
{
  double sign;
  if ( sector_ == 0 ) return;

  if ( speed_ >= 0.0 ) {
    sign = 1.0;
  } else {
    sign = -1.0;
  }
    
  track_->position(actual_speed_, pos_in_track_, sector_);
    
  if ( fabs(speed_) - fabs(actual_speed_) > 1e-6 ) { // speeding up
    actual_speed_ += sign*inertia_;
    if ( fabs(speed_) < fabs(actual_speed_) )
      actual_speed_ = speed_;

  } else if(fabs(speed_) - fabs(actual_speed_) < 1e-6) { // slowing down
    actual_speed_ -= sign*(inertia_+friction_);
    if (fabs(speed_) > fabs(actual_speed_))
      actual_speed_ = speed_;

  } else {
    actual_speed_ = speed_;
  }
}


Fake_controller::Fake_controller()
  : track_(),
    // following should be adjusted as necessary
    train1_(&track_, 3, 0.0),
    train2_(&track_, 5, 0.0),
    train4_(&track_, 12,0.0)
{
  train1_.set_speed(0);
  train2_.set_speed(0);
  train4_.set_speed(0);
}

USHORT Fake_controller::get_position_mask()
{
  USHORT posmask = 0;
  posmask |= (1 << (train1_.get_pos()-1));
  posmask |= (1 << (train2_.get_pos()-1));
  posmask |= (1 << (train4_.get_pos()-1));
  return posmask;
}

BOOL Fake_controller::set_train_speed(int num, SCHAR spd)
{
  switch(num) {
   case 1:
     train1_.set_speed(spd);
     return TRUE;
   case 2:
     train2_.set_speed(spd);
     return TRUE;
   case 4:
     train2_.set_speed(spd);
     return TRUE;
  }
  return FALSE;
}

BOOL Fake_controller::set_switch(int num, switchpoint_t pos)
{
  if ( num < 0 || num > 3 ) return FALSE;

  track_.switch_point(num, pos);
  return TRUE;
}

void Fake_controller::tick()
{
  train1_.tick();
  train2_.tick();
  train4_.tick();
}

char Fake_controller::next_position(char current_pos, SCHAR direction)
{
  char tpos = current_pos;
  double fine;

  // ensure we cross boundary
  if (direction > 0) {
    fine = 1.0;
  } else {
    fine = 0.0;
  }

  track_.position(direction, fine, tpos);
  return tpos;
}

#include "marshal.hh"
#include <time.h>

BEGIN_MESSAGE_TABLE(Fake_controller)
  MESSAGE(unmarshal_none_rushort,       &Fake_controller::get_position_mask)
  MESSAGE(unmarshal_int_schar_rbool,    &Fake_controller::set_train_speed  )
  MESSAGE(unmarshal_int_swpointt_rbool, &Fake_controller::set_switch       )
END_MESSAGE_TABLE;

#define MSG_get_position_mask 0
#define MSG_set_train_speed   1
#define MSG_set_switch        2

Fake_controller_proxy::Fake_controller_proxy(int msec, int alg, int prio)
{
  timespec ts;
  ts.tv_sec = msec/1000;
  ts.tv_usec = (msec%1000) * 1000;
  ctrl_.start(ts, alg, prio);
}

Fake_controller_proxy::~Fake_controller_proxy()
{
  ctrl_.stop();
}

USHORT Fake_controller_proxy::get_position_mask()
{
  return marshal_none_rushort(&ctrl_, MSG_get_position_mask);
}

BOOL Fake_controller_proxy::set_train_speed(int num, SCHAR spd)
{
    return marshal_int_schar_rbool(&ctrl_, MSG_set_train_speed, num, spd);
}

BOOL Fake_controller_proxy::set_switch(int num, switchpoint_t pos)
{
    return marshal_int_swpointt_rbool(&ctrl_, MSG_set_switch, num, pos);
}

#endif

---