Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Bugfix/minor stuff #51

Merged
merged 3 commits into from
Dec 1, 2023
Merged

Bugfix/minor stuff #51

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
8110caf
Source file doxygen names fixed. Avoid disabling differential drive u…
BlueAndi Dec 1, 2023
6911c5c
Button B and C handling in simulation fixed.
BlueAndi Dec 1, 2023
5911e50
Fixed calculation of linear speed left/right.
BlueAndi Dec 1, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 7 additions & 16 deletions lib/APPConvoyLeader/DrivingState.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@
DESCRIPTION
*******************************************************************************/
/**
* @brief Calibration state
* @brief Driving state
* @author Andreas Merkle <[email protected]>
*/

Expand Down Expand Up @@ -77,8 +77,6 @@ void DrivingState::entry()
m_trackStatus = TRACK_STATUS_ON_TRACK; /* Assume that the robot is placed on track. */
m_posMovAvg.clear();

diffDrive.enable();

/* Configure PID controller with selected parameter set. */
m_topSpeed = parSet.topSpeed;
m_pidCtrl.clear();
Expand Down Expand Up @@ -140,16 +138,17 @@ void DrivingState::process(StateMachine& sm)

void DrivingState::exit()
{
DifferentialDrive& diffDrive = DifferentialDrive::getInstance();

diffDrive.disable();
m_observationTimer.stop();
Board::getInstance().getYellowLed().enable(false);
}

/* PROTECTED METHODES *****************************************************************************/
/******************************************************************************
* Protected Methods
*****************************************************************************/

/* PRIVATE METHODES *******************************************************************************/
/******************************************************************************
* Private Methods
*****************************************************************************/

void DrivingState::processOnTrack(int16_t position, const uint16_t* lineSensorValues)
{
Expand Down Expand Up @@ -363,14 +362,6 @@ void DrivingState::adaptDriving(int16_t position)
diffDrive.setLinearSpeed(leftSpeed, rightSpeed);
}

/******************************************************************************
* Protected Methods
*****************************************************************************/

/******************************************************************************
* Private Methods
*****************************************************************************/

/******************************************************************************
* External Functions
*****************************************************************************/
Expand Down
2 changes: 1 addition & 1 deletion lib/APPConvoyLeader/ErrorState.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@
DESCRIPTION
*******************************************************************************/
/**
* @brief Calibration state
* @brief Error state
* @author Andreas Merkle <[email protected]>
*/

Expand Down
4 changes: 0 additions & 4 deletions lib/APPConvoyLeader/LineSensorsCalibrationState.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,6 @@ void LineSensorsCalibrationState::entry()
/* Prepare calibration drive. */
m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 3;
m_orientation = odometry.getOrientation();
diffDrive.enable();

/* Wait some time, before starting the calibration drive. */
m_phase = PHASE_1_WAIT;
Expand Down Expand Up @@ -134,9 +133,6 @@ void LineSensorsCalibrationState::process(StateMachine& sm)

void LineSensorsCalibrationState::exit()
{
DifferentialDrive& diffDrive = DifferentialDrive::getInstance();

diffDrive.disable();
m_timer.stop();
}

Expand Down
3 changes: 3 additions & 0 deletions lib/APPConvoyLeader/MotorSpeedCalibrationState.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -208,6 +208,9 @@ void MotorSpeedCalibrationState::finishCalibration(StateMachine& sm)
*/
diffDrive.setMaxMotorSpeed(maxSpeed);

/* Differential drive can now be used. */
diffDrive.enable();

if (0 == maxSpeed)
{
ErrorState::getInstance().setErrorMsg("MS=0");
Expand Down
2 changes: 1 addition & 1 deletion lib/APPConvoyLeader/ReadyState.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@
DESCRIPTION
*******************************************************************************/
/**
* @brief Calibration state
* @brief Ready state
* @author Andreas Merkle <[email protected]>
*/

Expand Down
23 changes: 7 additions & 16 deletions lib/APPLineFollower/DrivingState.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@
DESCRIPTION
*******************************************************************************/
/**
* @brief Calibration state
* @brief Driving state
* @author Andreas Merkle <[email protected]>
*/

Expand Down Expand Up @@ -76,8 +76,6 @@ void DrivingState::entry()
m_lineStatus = LINE_STATUS_FIND_START_LINE;
m_trackStatus = TRACK_STATUS_ON_TRACK; /* Assume that the robot is placed on track. */

diffDrive.enable();

/* Configure PID controller with selected parameter set. */
m_topSpeed = parSet.topSpeed;
m_pidCtrl.clear();
Expand Down Expand Up @@ -137,16 +135,17 @@ void DrivingState::process(StateMachine& sm)

void DrivingState::exit()
{
DifferentialDrive& diffDrive = DifferentialDrive::getInstance();

diffDrive.disable();
m_observationTimer.stop();
Board::getInstance().getYellowLed().enable(false);
}

/* PROTECTED METHODES *****************************************************************************/
/******************************************************************************
* Protected Methods
*****************************************************************************/

/* PRIVATE METHODES *******************************************************************************/
/******************************************************************************
* Private Methods
*****************************************************************************/

void DrivingState::processOnTrack(int16_t position, const uint16_t* lineSensorValues)
{
Expand Down Expand Up @@ -361,14 +360,6 @@ void DrivingState::adaptDriving(int16_t position)
diffDrive.setLinearSpeed(leftSpeed, rightSpeed);
}

/******************************************************************************
* Protected Methods
*****************************************************************************/

/******************************************************************************
* Private Methods
*****************************************************************************/

/******************************************************************************
* External Functions
*****************************************************************************/
Expand Down
2 changes: 1 addition & 1 deletion lib/APPLineFollower/ErrorState.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@
DESCRIPTION
*******************************************************************************/
/**
* @brief Calibration state
* @brief Error state
* @author Andreas Merkle <[email protected]>
*/

Expand Down
4 changes: 0 additions & 4 deletions lib/APPLineFollower/LineSensorsCalibrationState.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,6 @@ void LineSensorsCalibrationState::entry()
/* Prepare calibration drive. */
m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 3;
m_orientation = odometry.getOrientation();
diffDrive.enable();

/* Wait some time, before starting the calibration drive. */
m_phase = PHASE_1_WAIT;
Expand Down Expand Up @@ -134,9 +133,6 @@ void LineSensorsCalibrationState::process(StateMachine& sm)

void LineSensorsCalibrationState::exit()
{
DifferentialDrive& diffDrive = DifferentialDrive::getInstance();

diffDrive.disable();
m_timer.stop();
}

Expand Down
3 changes: 3 additions & 0 deletions lib/APPLineFollower/MotorSpeedCalibrationState.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -208,6 +208,9 @@ void MotorSpeedCalibrationState::finishCalibration(StateMachine& sm)
*/
diffDrive.setMaxMotorSpeed(maxSpeed);

/* Differential drive can now be used. */
diffDrive.enable();

if (0 == maxSpeed)
{
ErrorState::getInstance().setErrorMsg("MS=0");
Expand Down
2 changes: 1 addition & 1 deletion lib/APPLineFollower/ReadyState.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@
DESCRIPTION
*******************************************************************************/
/**
* @brief Calibration state
* @brief Ready state
* @author Andreas Merkle <[email protected]>
*/

Expand Down
2 changes: 1 addition & 1 deletion lib/APPLineFollower/ReleaseTrackState.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@
DESCRIPTION
*******************************************************************************/
/**
* @brief Calibration state
* @brief Release track state
* @author Andreas Merkle <[email protected]>
*/

Expand Down
2 changes: 1 addition & 1 deletion lib/APPRemoteControl/ErrorState.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@
DESCRIPTION
*******************************************************************************/
/**
* @brief Calibration state
* @brief Error state
* @author Andreas Merkle <[email protected]>
*/

Expand Down
4 changes: 0 additions & 4 deletions lib/APPRemoteControl/LineSensorsCalibrationState.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,6 @@ void LineSensorsCalibrationState::entry()
/* Prepare calibration drive. */
m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 3;
m_orientation = odometry.getOrientation();
diffDrive.enable();

/* Wait some time, before starting the calibration drive. */
m_phase = PHASE_1_WAIT;
Expand Down Expand Up @@ -134,9 +133,6 @@ void LineSensorsCalibrationState::process(StateMachine& sm)

void LineSensorsCalibrationState::exit()
{
DifferentialDrive& diffDrive = DifferentialDrive::getInstance();

diffDrive.disable();
m_timer.stop();
}

Expand Down
3 changes: 3 additions & 0 deletions lib/APPRemoteControl/MotorSpeedCalibrationState.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -208,6 +208,9 @@ void MotorSpeedCalibrationState::finishCalibration(StateMachine& sm)
*/
diffDrive.setMaxMotorSpeed(maxSpeed);

/* Differential drive can now be used. */
diffDrive.enable();

if (0 == maxSpeed)
{
ErrorState::getInstance().setErrorMsg("MS=0");
Expand Down
4 changes: 2 additions & 2 deletions lib/HALSim/ButtonB.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,12 +60,12 @@

bool ButtonB::isPressed()
{
return m_keyboard.buttonAPressed();
return m_keyboard.buttonBPressed();
}

void ButtonB::waitForRelease()
{
m_keyboard.waitForReleaseA();
m_keyboard.waitForReleaseB();
}

/******************************************************************************
Expand Down
4 changes: 2 additions & 2 deletions lib/HALSim/ButtonC.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,12 +60,12 @@

bool ButtonC::isPressed()
{
return m_keyboard.buttonAPressed();
return m_keyboard.buttonCPressed();
}

void ButtonC::waitForRelease()
{
m_keyboard.waitForReleaseA();
m_keyboard.waitForReleaseC();
}

/******************************************************************************
Expand Down
60 changes: 50 additions & 10 deletions lib/Service/DifferentialDrive.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,36 @@
* Public Methods
*****************************************************************************/

void DifferentialDrive::enable()
{
m_linearSpeedCenterSetPoint = 0;
m_linearSpeedLeftSetPoint = 0;
m_linearSpeedRightSetPoint = 0;
m_angularSpeedSetPoint = 0;

m_motorSpeedLeftPID.clear();
m_motorSpeedRightPID.clear();

m_isEnabled = true;
}

void DifferentialDrive::disable()
{
IMotors& motors = Board::getInstance().getMotors();

/* Ensure that the motors are stopped, otherwise it may happen that the
* set points of 0 by the PID controllers are not reached yet.
*/
motors.setSpeeds(0, 0);

m_linearSpeedCenterSetPoint = 0;
m_linearSpeedLeftSetPoint = 0;
m_linearSpeedRightSetPoint = 0;
m_angularSpeedSetPoint = 0;

m_isEnabled = false;
}

int16_t DifferentialDrive::getMaxMotorSpeed() const
{
return m_maxMotorSpeed;
Expand Down Expand Up @@ -107,7 +137,7 @@ int16_t DifferentialDrive::getAngularSpeed() const

void DifferentialDrive::setAngularSpeed(int16_t angularSpeed)
{
m_angularSpeedSetPoint = angularSpeed;
m_angularSpeedSetPoint = constrain(angularSpeed, -m_maxMotorSpeed, m_maxMotorSpeed);
calculateLinearSpeedLeftRight(m_linearSpeedCenterSetPoint, m_angularSpeedSetPoint, m_linearSpeedLeftSetPoint,
m_linearSpeedRightSetPoint);
}
Expand Down Expand Up @@ -193,13 +223,18 @@ void DifferentialDrive::process(uint32_t period)
void DifferentialDrive::calculateLinearSpeedLeftRight(int16_t linearSpeedCenter, int16_t angularSpeed,
int16_t& linearSpeedLeft, int16_t& linearSpeedRight)
{
int32_t linearSpeedCenter32 = static_cast<int32_t>(linearSpeedCenter); /* [steps/s] */
int32_t angularSpeed32 = static_cast<int32_t>(angularSpeed); /* [mrad/s] */
int32_t wheelDiameter32 = static_cast<int32_t>(RobotConstants::WHEEL_DIAMETER); /* [mm] */
int32_t wheelBase32 = static_cast<int32_t>(RobotConstants::WHEEL_BASE); /* [mm] */
int32_t linearSpeedCenter32 = static_cast<int32_t>(linearSpeedCenter); /* [steps/s] */
int32_t angularSpeed32 = static_cast<int32_t>(angularSpeed); /* [mrad/s] */
int32_t wheelBase32 = static_cast<int32_t>(RobotConstants::WHEEL_BASE); /* [mm] */

/* angular speed = 2 * (linear speed right - linear speed left ) / wheel base
* linear speed right - linear speed left = angular speed * wheel base / 2
*
* linear speed right = - linear speed left
*/

linearSpeedLeft = (linearSpeedCenter32 * 2 - (angularSpeed32 * wheelBase32) / 1000) / wheelDiameter32;
linearSpeedRight = (linearSpeedCenter32 * 2 + (angularSpeed32 * wheelBase32) / 1000) / wheelDiameter32;
linearSpeedLeft = linearSpeedCenter32 - (angularSpeed32 * wheelBase32) / 2;
linearSpeedRight = linearSpeedCenter32 + (angularSpeed32 * wheelBase32) / 2;
}

void DifferentialDrive::calculateLinearAndAngularSpeedCenter(int16_t linearSpeedLeft, int16_t linearSpeedRight,
Expand All @@ -209,9 +244,14 @@ void DifferentialDrive::calculateLinearAndAngularSpeedCenter(int16_t linearSpeed
int32_t linearSpeedRight32 = static_cast<int32_t>(linearSpeedRight); /* [steps/s] */
int32_t wheelDiameter32 = static_cast<int32_t>(RobotConstants::WHEEL_DIAMETER); /* [mm] */
int32_t wheelBase32 = static_cast<int32_t>(RobotConstants::WHEEL_BASE); /* [mm] */
int32_t linearSpeedCenter32 = (wheelDiameter32 * (linearSpeedRight32 + linearSpeedLeft32)) / 4; /* [steps/s] */
int32_t angularSpeed32 =
(wheelDiameter32 * (linearSpeedRight32 - linearSpeedLeft32) * 1000) / wheelBase32; /* [mrad/s] */
int32_t linearSpeedCenter32 = (wheelDiameter32 * (linearSpeedRight32 + linearSpeedLeft32)) / 2; /* [steps/s] */
int32_t angularSpeed32 = (2 * (linearSpeedRight32 - linearSpeedLeft32)) / wheelBase32; /* [mrad/s] */

/* linear speed = (wheel radius / 2) * (linear speed right + linear speed left)
* linear speed = (wheel radius * (linear speed right + linear speed left)) / 2
*
* angular speed = 2 * (linear speed right - linear speed left ) / wheel base
*/

linearSpeedCenter = static_cast<int16_t>(linearSpeedCenter32);
angularSpeed = static_cast<int16_t>(angularSpeed32);
Expand Down
Loading