diff --git a/lib/APPLineFollower/DrivingState.cpp b/lib/APPLineFollower/DrivingState.cpp index ecb6f275..c64be1ff 100644 --- a/lib/APPLineFollower/DrivingState.cpp +++ b/lib/APPLineFollower/DrivingState.cpp @@ -75,7 +75,6 @@ void DrivingState::entry() m_pidProcessTime.start(0); /* Immediate */ m_lineStatus = LINE_STATUS_FIND_START_LINE; m_trackStatus = TRACK_STATUS_ON_TRACK; /* Assume that the robot is placed on track. */ - m_posMovAvg.clear(); diffDrive.enable(); @@ -99,8 +98,6 @@ void DrivingState::process(StateMachine& sm) /* Get the position of the line. */ position = lineSensors.readLine(); - (void)m_posMovAvg.write(position); - switch (m_trackStatus) { case TRACK_STATUS_ON_TRACK: @@ -159,7 +156,7 @@ void DrivingState::processOnTrack(int16_t position, const uint16_t* lineSensorVa } /* Track lost just in this moment? */ - if (true == isTrackGapDetected(m_posMovAvg.getResult())) + if (true == isTrackGapDetected(position)) { m_trackStatus = TRACK_STATUS_LOST; @@ -332,6 +329,7 @@ void DrivingState::adaptDriving(int16_t position) { DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); const ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + const int16_t MAX_MOTOR_SPEED = diffDrive.getMaxMotorSpeed(); int16_t speedDifference = 0; /* [steps/s] */ int16_t leftSpeed = 0; /* [steps/s] */ int16_t rightSpeed = 0; /* [steps/s] */ @@ -357,8 +355,8 @@ void DrivingState::adaptDriving(int16_t position) * might want to allow the motor speed to go negative so that * it can spin in reverse. */ - leftSpeed = constrain(leftSpeed, 0, m_topSpeed); - rightSpeed = constrain(rightSpeed, 0, m_topSpeed); + leftSpeed = constrain(leftSpeed, 0, MAX_MOTOR_SPEED); + rightSpeed = constrain(rightSpeed, 0, MAX_MOTOR_SPEED); diffDrive.setLinearSpeed(leftSpeed, rightSpeed); } diff --git a/lib/APPLineFollower/DrivingState.h b/lib/APPLineFollower/DrivingState.h index 45433d88..ad735820 100644 --- a/lib/APPLineFollower/DrivingState.h +++ b/lib/APPLineFollower/DrivingState.h @@ -47,7 +47,6 @@ #include #include #include -#include /****************************************************************************** * Macros @@ -131,7 +130,6 @@ class DrivingState : public IState LineStatus m_lineStatus; /**< Status of start-/end line detection */ TrackStatus m_trackStatus; /**< Status of track which means on track or track lost, etc. */ uint8_t m_startEndLineDebounce; /**< Counter used for easys debouncing of the start-/end line detection. */ - MovAvg m_posMovAvg; /**< The moving average of the position over 2 calling cycles. */ /** * Default constructor. @@ -144,8 +142,7 @@ class DrivingState : public IState m_topSpeed(0), m_lineStatus(LINE_STATUS_FIND_START_LINE), m_trackStatus(TRACK_STATUS_ON_TRACK), - m_startEndLineDebounce(0), - m_posMovAvg() + m_startEndLineDebounce(0) { } diff --git a/lib/APPLineFollower/ParameterSets.cpp b/lib/APPLineFollower/ParameterSets.cpp index fd36c3e7..49042585 100644 --- a/lib/APPLineFollower/ParameterSets.cpp +++ b/lib/APPLineFollower/ParameterSets.cpp @@ -110,7 +110,7 @@ ParameterSets::ParameterSets() : m_currentSetId(0), m_parSets() 2, /* Kp Denominator */ 1, /* Ki Numerator */ 40, /* Ki Denominator */ - 10, /* Kd Numerator */ + 40, /* Kd Numerator */ 1 /* Kd Denominator */ }; @@ -121,7 +121,7 @@ ParameterSets::ParameterSets() : m_currentSetId(0), m_parSets() 1, /* Kp Denominator */ 0, /* Ki Numerator */ 1, /* Ki Denominator */ - 10, /* Kd Numerator */ + 40, /* Kd Numerator */ 1 /* Kd Denominator */ }; } diff --git a/lib/ArduinoNative/Arduino.cpp b/lib/ArduinoNative/Arduino.cpp index d7a6f09d..52c9d48a 100644 --- a/lib/ArduinoNative/Arduino.cpp +++ b/lib/ArduinoNative/Arduino.cpp @@ -240,16 +240,26 @@ extern int main(int argc, char** argv) * https://cyberbotics.com/doc/reference/robot#synchronous-versus-asynchronous-controllers */ - setup(); - - while (true == gSimTime->step()) + /* Do one single simulation step to force that all sensors will deliver already data. + * Otherwise e.g. the position sensor will provide NaN. + * This must be done before setup() is called! + */ + if (false == gSimTime->step()) { - keyboard.getPressedButtons(); - loop(); - gSocketStream.process(); + printf("Very first simulation step failed.\n"); + status = -1; } + else + { + setup(); - status = 0; + while (true == gSimTime->step()) + { + keyboard.getPressedButtons(); + loop(); + gSocketStream.process(); + } + } } return status; diff --git a/lib/HALSim/Encoders.cpp b/lib/HALSim/Encoders.cpp index 4673e25a..4b401fea 100644 --- a/lib/HALSim/Encoders.cpp +++ b/lib/HALSim/Encoders.cpp @@ -64,11 +64,21 @@ void Encoders::init() if (nullptr != m_posSensorLeft) { m_posSensorLeft->enable(m_simTime.getTimeStep()); + + /* If robot reconnets, the position sensor will provide its position. + * Ensure that the left encoder will start at 0. + */ + m_lastResetValueLeft = m_posSensorLeft->getValue(); } if (nullptr != m_posSensorRight) { m_posSensorRight->enable(m_simTime.getTimeStep()); + + /* If robot reconnets, the position sensor will provide its position. + * Ensure that the right encoder will start at 0. + */ + m_lastResetValueRight = m_posSensorRight->getValue(); } } @@ -101,8 +111,8 @@ int16_t Encoders::getCountsRight() { double currentPos = m_posSensorRight->getValue(); - overflowProtection(m_lastResetValueLeft, currentPos); - steps = calculateSteps(m_lastResetValueLeft, currentPos); + overflowProtection(m_lastResetValueRight, currentPos); + steps = calculateSteps(m_lastResetValueRight, currentPos); } return steps;