120 lines
2.7 KiB
C++
120 lines
2.7 KiB
C++
#include<locomotive.h>
|
|
#include<motor.h>
|
|
#include<math.h>
|
|
|
|
Locomotive::Locomotive(int accelerate, int decelerate, byte minSpeed, byte maxSpeed, IMotor& motor): _motor(motor)
|
|
{
|
|
_currentSpeed=0;
|
|
_currentAcc=0;
|
|
_accelerate=accelerate;
|
|
_decelerate=decelerate;
|
|
_minSpeed=minSpeed;
|
|
_maxSpeed=maxSpeed;
|
|
|
|
};
|
|
|
|
void Locomotive::Accelerate()
|
|
{
|
|
Accelerate(_maxSpeed);
|
|
};
|
|
|
|
void Locomotive::Accelerate(byte Speed)
|
|
{
|
|
/* Serial.print("Locomotive.Accelerate: ");
|
|
Serial.print(_currentSpeed);
|
|
Serial.print(" ");
|
|
Serial.print(Speed);
|
|
Serial.print(" ");
|
|
*/
|
|
_targetSpeed = Speed;
|
|
_millisStart = millis()-_delay;
|
|
if (_targetSpeed>_currentSpeed)
|
|
_currentAcc = _accelerate;
|
|
else
|
|
_currentAcc = - _decelerate;
|
|
|
|
};
|
|
|
|
|
|
void Locomotive::Accelerate(byte Speed, float bremsweg)
|
|
{
|
|
_targetSpeed = Speed;
|
|
_currentSpeed = (_currentSpeed<_minSpeed)? _minSpeed : _currentSpeed;
|
|
_millisStart = millis()-_delay;
|
|
|
|
byte _targetSpeed2 = ((_targetSpeed < _currentSpeed)&&(_targetSpeed == 0)) ? _minSpeed : _targetSpeed;
|
|
int deltaspeed = (_targetSpeed2 - _currentSpeed);
|
|
|
|
_currentAcc = copysign(deltaspeed * deltaspeed / (100.0 * bremsweg), deltaspeed);
|
|
if (_currentAcc == 0)
|
|
_currentAcc = copysign(1, deltaspeed);
|
|
|
|
};
|
|
|
|
|
|
void Locomotive::Break()
|
|
{
|
|
Accelerate(0);
|
|
};
|
|
|
|
void Locomotive::setSpeed(byte 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 = _minSpeed * 80 / 100;
|
|
|
|
if ((newspeed>_targetSpeed) && (_currentAcc>0))
|
|
{
|
|
newspeed = _targetSpeed;
|
|
_currentAcc =0;
|
|
}
|
|
else if ((newspeed<_minSpeed) && (_currentAcc>0)) newspeed = _minSpeed;
|
|
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.
|
|
}
|
|
|
|
};
|
|
|
|
byte Locomotive::currentSpeed()
|
|
{
|
|
return ((byte) (_currentSpeed+0.5));
|
|
};
|
|
|
|
byte Locomotive::getDefaultSpeed()
|
|
{
|
|
return _maxSpeed;
|
|
};
|
|
|