From 0cb90a4e9ae7eee20fd0f1421032e7ebb29b0529 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Kr=C3=B3lik=20Uszasty?= <bunny@awril.pl>
Date: Sun, 15 Apr 2018 14:58:51 +0200
Subject: [PATCH] Improved speed controller - speed can be selected by
 SecondControlPosition

---
 Driver.cpp         | 20 ++++++++++++++++++--
 Driver.h           |  1 +
 McZapkie/MOVER.h   |  3 +++
 McZapkie/Mover.cpp | 35 ++++++++++++++++++++++++++++++++---
 4 files changed, 54 insertions(+), 5 deletions(-)

diff --git a/Driver.cpp b/Driver.cpp
index eb44fdf0..1c2522ef 100644
--- a/Driver.cpp
+++ b/Driver.cpp
@@ -2728,13 +2728,14 @@ bool TController::IncSpeed()
             {
                 OK = mvControlling->IncMainCtrl(std::max(1,mvOccupied->MainCtrlPosNo/10));
 				//tutaj jeszcze powinien być tempomat
-				mvControlling->IncScndCtrl(1);
+
 				double SpeedCntrl = VelDesired;
 				if (fProximityDist < 50)
 				{
 					SpeedCntrl = std::min(SpeedCntrl, VelNext);
 				}
-				mvControlling->RunCommand("SpeedCntrl", VelDesired, mvControlling->CabNo);
+				this->SpeedCntrl(SpeedCntrl);
+
             }
         break;
     case WheelsDriven:
@@ -3011,6 +3012,21 @@ void TController::SpeedSet()
     }
 };
 
+void TController::SpeedCntrl(double DesiredSpeed)
+{
+	if (mvControlling->ScndCtrlPosNo == 1)
+	{
+		mvControlling->IncScndCtrl(1);
+		mvControlling->RunCommand("SpeedCntrl", DesiredSpeed, mvControlling->CabNo);
+	}
+	else if (mvControlling->ScndCtrlPosNo > 1)
+	{
+		int DesiredPos = 1 + mvControlling->ScndCtrlPosNo * ((DesiredSpeed - 1.0) / mvControlling->Vmax);
+		while (mvControlling->ScndCtrlPos > DesiredPos) mvControlling->DecScndCtrl(1);
+		while (mvControlling->ScndCtrlPos < DesiredPos) mvControlling->IncScndCtrl(1);
+	}
+};
+
 // otwieranie/zamykanie drzwi w składzie albo (tylko AI) EZT
 void TController::Doors( bool const Open, int const Side ) {
 
diff --git a/Driver.h b/Driver.h
index 214df307..95277696 100644
--- a/Driver.h
+++ b/Driver.h
@@ -304,6 +304,7 @@ private:
     bool IncSpeed();
     bool DecSpeed(bool force = false);
     void SpeedSet();
+	void SpeedCntrl(double DesiredSpeed);
     void Doors(bool const Open, int const Side = 0);
     // returns true if any vehicle in the consist has an open door
     bool doors_open() const;
diff --git a/McZapkie/MOVER.h b/McZapkie/MOVER.h
index 97231ad9..a67278b5 100644
--- a/McZapkie/MOVER.h
+++ b/McZapkie/MOVER.h
@@ -940,6 +940,7 @@ public:
 	double PlatformMaxShift = 0.5; /*wysuniecie stopnia*/
 	int PlatformOpenMethod = 1; /*sposob animacji stopnia*/
 	bool ScndS = false; /*Czy jest bocznikowanie na szeregowej*/
+	double SpeedCtrlDelay = 2; /*opoznienie dzialania tempomatu z wybieralna predkoscia*/
     /*--sekcja zmiennych*/
     /*--opis konkretnego egzemplarza taboru*/
 	TLocation Loc; //pozycja pojazdów do wyznaczenia odległości pomiędzy sprzęgami
@@ -1127,6 +1128,8 @@ public:
     /*- zmienne dla lokomotyw z silnikami indukcyjnymi -*/
 	double eimv[21];
     static std::vector<std::string> const eimv_labels;
+	double SpeedCtrlTimer = 0; /*zegar dzialania tempomatu z wybieralna predkoscia*/
+	double NewSpeed = 0; /*nowa predkosc do zadania*/
 
 	/*-zmienne dla drezyny*/
 	double PulseForce = 0.0;        /*przylozona sila*/
diff --git a/McZapkie/Mover.cpp b/McZapkie/Mover.cpp
index b340ca79..9a5200f0 100644
--- a/McZapkie/Mover.cpp
+++ b/McZapkie/Mover.cpp
@@ -2002,7 +2002,7 @@ bool TMoverParameters::IncScndCtrl(int CtrlSpeed)
         if (LastRelayTime > CtrlDelay)
             LastRelayTime = 0;
 
-	if ((OK) && (EngineType == ElectricInductionMotor))
+	if ((OK) && (EngineType == ElectricInductionMotor) && (ScndCtrlPosNo == 1))
 	{
 		// NOTE: round() already adds 0.5, are the ones added here as well correct?
 		if ((Vmax < 250))
@@ -2060,7 +2060,7 @@ bool TMoverParameters::DecScndCtrl(int CtrlSpeed)
         if (LastRelayTime > CtrlDownDelay)
             LastRelayTime = 0;
 
-	if ((OK) && (EngineType == ElectricInductionMotor))
+	if ((OK) && (EngineType == ElectricInductionMotor) && (ScndCtrlPosNo == 1))
 	{
 		ScndCtrlActualPos = 0;
 		SendCtrlToNext("SpeedCntrl", ScndCtrlActualPos, CabNo);
@@ -4992,6 +4992,32 @@ double TMoverParameters::TractionForce(double dt)
             }
             if( true == Mains ) {
 
+				//tempomat
+
+				if (ScndCtrlPosNo > 1)
+				{
+					if (ScndCtrlPos != NewSpeed)
+					{
+
+						SpeedCtrlTimer = 0;
+						NewSpeed = ScndCtrlPos;
+					}
+					else
+					{
+						SpeedCtrlTimer += dt;
+						if (SpeedCtrlTimer > SpeedCtrlDelay)
+						{
+							int NewSCAP = (Vmax < 250 ? 1 : 0.5) * (float)ScndCtrlPos / (float)ScndCtrlPosNo * Vmax;
+							if (NewSCAP != ScndCtrlActualPos)
+							{
+								ScndCtrlActualPos = NewSCAP;
+								SendCtrlToNext("SpeedCntrl", ScndCtrlActualPos, CabNo);
+							}
+						}
+					}
+				}
+
+
                 dtrans = Hamulec->GetEDBCP();
                 if (((DoorLeftOpened) || (DoorRightOpened)))
                     DynamicBrakeFlag = true;
@@ -5031,7 +5057,7 @@ double TMoverParameters::TractionForce(double dt)
                     eimv[eimv_Fzad] = PosRatio;
                     if ((Flat) && (eimc[eimc_p_F0] * eimv[eimv_Fful] > 0))
                         PosRatio = Min0R(PosRatio * eimc[eimc_p_F0] / eimv[eimv_Fful], 1);
-                    if (ScndCtrlActualPos > 0)
+                    if (ScndCtrlActualPos > 0) //speed control
                         if (Vmax < 250)
                             PosRatio = Min0R(PosRatio, Max0R(-1, 0.5 * (ScndCtrlActualPos - Vel)));
                         else
@@ -8093,6 +8119,9 @@ void TMoverParameters::LoadFIZ_Cntrl( std::string const &line ) {
 
     extract_value( StopBrakeDecc, "SBD", line, "" );
 
+    // speed control
+    extract_value( SpeedCtrlDelay, "SpeedCtrlDelay", line, "" );
+
     // converter
     {
         std::map<std::string, start> starts {