handynet
fe94db4e88
Umstellung auf Float bei Beschleunigung Entfernen von minspeed Entfernen nicht notwendiger Konstruktoren in Lokomotive
125 lines
3.2 KiB
C++
125 lines
3.2 KiB
C++
#include<locomotive.h>
|
|
#include<motor.h>
|
|
#include<math.h>
|
|
|
|
Locomotive::Locomotive(float accelerate, float decelerate, byte minSpeedStart, byte minSpeedStop, IMotor& motor): _motor(motor)
|
|
{
|
|
_currentSpeed=0;
|
|
_currentAcc=0;
|
|
_accelerate=accelerate;
|
|
_decelerate=decelerate;
|
|
_minSpeedStart=minSpeedStart;
|
|
_minSpeedStop=minSpeedStop;
|
|
|
|
};
|
|
|
|
void Locomotive::Accelerate(byte Speed)
|
|
{
|
|
/* Serial.print("Locomotive.Accelerate: ");
|
|
Serial.print(_currentSpeed);
|
|
Serial.print(" ");
|
|
Serial.print(Speed);
|
|
Serial.print(" ");
|
|
*/
|
|
Serial.print("Accelerate: currentspeed=");
|
|
Serial.print((_currentSpeed));
|
|
Serial.print(" targetspeed=");
|
|
Serial.print(Speed);
|
|
|
|
_targetSpeed = Speed;
|
|
_millisStart = millis()-_delay;
|
|
if (_targetSpeed>_currentSpeed)
|
|
_currentAcc = _accelerate;
|
|
else
|
|
_currentAcc = - _decelerate;
|
|
|
|
Serial.print(" _currentAcc=");
|
|
Serial.print(_currentAcc);
|
|
Serial.println();
|
|
|
|
};
|
|
|
|
|
|
void Locomotive::Accelerate(byte Speed, float bremsweg)
|
|
{
|
|
Serial.print("Accelerate: currentspeed=");
|
|
Serial.print((_currentSpeed));
|
|
Serial.print(" targetspeed=");
|
|
Serial.print(Speed);
|
|
Serial.print(" bremsweg=");
|
|
Serial.print((bremsweg));
|
|
|
|
_targetSpeed = Speed;
|
|
_currentSpeed = (_currentSpeed<_minSpeedStart)? _minSpeedStart : _currentSpeed;
|
|
_millisStart = millis()-_delay;
|
|
|
|
byte _targetSpeed2 = ((_targetSpeed < _currentSpeed)&&(_targetSpeed == 0)) ? _minSpeedStop : _targetSpeed;
|
|
int deltaspeed = (_targetSpeed2 - _currentSpeed);
|
|
|
|
_currentAcc = copysign(deltaspeed * deltaspeed / (100.0 * bremsweg ) * (_delay / 300.0) , deltaspeed);
|
|
if (_currentAcc == 0)
|
|
_currentAcc = copysign(1, deltaspeed);
|
|
|
|
Serial.print(" targetspeed2=");
|
|
Serial.print(_targetSpeed2);
|
|
Serial.print(" _currentAcc=");
|
|
Serial.print(_currentAcc);
|
|
Serial.println();
|
|
};
|
|
|
|
void Locomotive::setSpeed(float Speed)
|
|
{
|
|
_currentSpeed = Speed;
|
|
_motor.setSpeed( (int) (_currentSpeed+0.5));
|
|
};
|
|
|
|
void Locomotive::setDirection(MotorDirection dir)
|
|
{
|
|
Serial.print("Lokomotive Richtung: ");
|
|
Serial.println(dir);
|
|
_motor.setDirection(dir);
|
|
};
|
|
|
|
void Locomotive::loop()
|
|
{
|
|
unsigned long currentMillis;
|
|
currentMillis = millis();
|
|
if (currentMillis - _millisStart >= _delay) //test whether the period has elapsed
|
|
{
|
|
float newspeed;
|
|
newspeed = _currentSpeed + _currentAcc;
|
|
/* Serial.print("Locomotive.Loop: ");
|
|
Serial.print(_currentSpeed);
|
|
Serial.print(" ");
|
|
Serial.print(_currentAcc);
|
|
Serial.print(" ");
|
|
Serial.println(newspeed);
|
|
*/
|
|
|
|
int breakspeed = _minSpeedStop;
|
|
|
|
if ((newspeed>_targetSpeed) && (_currentAcc>0))
|
|
{
|
|
newspeed = _targetSpeed;
|
|
_currentAcc =0;
|
|
}
|
|
else if ((newspeed<_minSpeedStart) && (_currentAcc>0)) newspeed = _minSpeedStart;
|
|
else if ((newspeed<breakspeed) && (_currentAcc<0))
|
|
{
|
|
newspeed = 0;
|
|
_currentAcc = 0;
|
|
};
|
|
|
|
setSpeed(newspeed);
|
|
|
|
_millisStart = currentMillis; //IMPORTANT to save the start time of the current LED state.
|
|
}
|
|
|
|
};
|
|
|
|
float Locomotive::currentSpeed()
|
|
{
|
|
return (_currentSpeed);
|
|
};
|
|
|