Skip to content

Commit

Permalink
fixed review findings
Browse files Browse the repository at this point in the history
  • Loading branch information
@jkerpe
jkerpe committed Jun 26, 2024
1 parent ff181d7 commit 97dd907
Show file tree
Hide file tree
Showing 3 changed files with 1 addition and 180 deletions.
2 changes: 1 addition & 1 deletion README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -156,7 +156,7 @@ Example for the **LineFollowerTarget** application:
| ConvoyFollower | Convoy follower, providing information to the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip) to drive to its target. | No | Yes | ./webots/worlds/zumo_with_com_system/PlatoonTrack.wbt |
| LineFollower | Just a line follower, using a PID controller. | Yes | No | ./webots/worlds/ETrack.wbt ./webots/worlds/LargeTrack.wbt ./webots/worlds/LineFollowerTrack.wbt |
| RemoteControl | The robot is remote controlled by e.g. the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip) in a convoy follower role. | No | Yes | ./webots/world/zumo_with_com_system/* |
| SensorFusion | The robot provides odometry and inertial data to the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip), which calculates the sensor fusion based location information. | No | Yes | ./webots/world/zumo_with_com_system/LineFollowerTrack.wbt |
| SensorFusion | The robot provides odometry and inertial data to the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip), which calculates the sensor fusion based location information. | No | Yes | ./webots/worlds/zumo_with_com_system/LineFollowerTrack.wbt |
| Test | Only for testing purposes on native environment. | Yes | No | N/A |

# Documentation
Expand Down
169 changes: 0 additions & 169 deletions lib/APPSensorFusion/src/DrivingState.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,23 +58,10 @@
* Prototypes
*****************************************************************************/

#ifdef DEBUG_ALGORITHM
static void logCsvDataTitles(uint8_t length);
static void logCsvDataTimestamp();
static void logCsvData(const uint16_t* lineSensorValues, uint8_t length, int16_t pos, int16_t pos3, bool isPos3Valid);
void logCsvDataTrackStatus(DrivingState::TrackStatus trackStatus);
static void logCsvDataSpeed(int16_t leftSpeed, int16_t rightSpeed);
#endif /* DEBUG_ALGORITHM */

/******************************************************************************
* Local Variables
*****************************************************************************/

#ifdef DEBUG_ALGORITHM
static int16_t gSpeedLeft = 0;
static int16_t gSpeedRight = 0;
#endif /* DEBUG_ALGORITHM */

const uint16_t DrivingState::SENSOR_VALUE_MAX = Board::getInstance().getLineSensors().getSensorValueMax();

/* Calculate the position set point to be generic. */
Expand Down Expand Up @@ -119,10 +106,6 @@ void DrivingState::entry()
m_pidCtrl.setSampleTime(PID_PROCESS_PERIOD);
m_pidCtrl.setLimits(-maxSpeed, maxSpeed);
m_pidCtrl.setDerivativeOnMeasurement(false);

#ifdef DEBUG_ALGORITHM
logCsvDataTitles(Board::getInstance().getLineSensors().getNumLineSensors());
#endif /* DEBUG_ALGORITHM */
}

void DrivingState::process(StateMachine& sm)
Expand All @@ -140,11 +123,6 @@ void DrivingState::process(StateMachine& sm)
bool isPosition3Valid = calcPosition3(position3, lineSensorValues, numLineSensors);
bool isTrackLost = isNoLineDetected(lineSensorValues, numLineSensors);

#ifdef DEBUG_ALGORITHM
logCsvDataTimestamp();
logCsvData(lineSensorValues, numLineSensors, position, position3, isPosition3Valid);
#endif /* DEBUG_ALGORITHM */

/* If the position calculated with the inner sensors is not valid, the
* position will be taken.
*/
Expand Down Expand Up @@ -281,12 +259,6 @@ void DrivingState::process(StateMachine& sm)
sm.setState(&ReadyState::getInstance());
}

#ifdef DEBUG_ALGORITHM
logCsvDataTrackStatus(nextTrackStatus);
logCsvDataSpeed(gSpeedLeft, gSpeedRight);
printf("\n");
#endif /* DEBUG_ALGORITHM */

/* Take over values for next cycle. */
m_trackStatus = nextTrackStatus;
m_isTrackLost = isTrackLost;
Expand Down Expand Up @@ -716,11 +688,6 @@ void DrivingState::adaptDriving(int16_t position, bool allowNegativeMotorSpeed)
leftSpeed = constrain(leftSpeed, MIN_MOTOR_SPEED, MAX_MOTOR_SPEED);
rightSpeed = constrain(rightSpeed, MIN_MOTOR_SPEED, MAX_MOTOR_SPEED);

#ifdef DEBUG_ALGORITHM
gSpeedLeft = leftSpeed;
gSpeedRight = rightSpeed;
#endif /* DEBUG_ALGORITHM */

diffDrive.setLinearSpeed(leftSpeed, rightSpeed);
}

Expand Down Expand Up @@ -757,139 +724,3 @@ bool DrivingState::isAbortRequired()
/******************************************************************************
* Local Functions
*****************************************************************************/

#ifdef DEBUG_ALGORITHM

/**
* Log the titles of each column as CSV data.
*
* @param[in] length Number of line sensors.
*/
static void logCsvDataTitles(uint8_t length)
{
uint8_t idx = 0;

printf("Timestamp");

while (length > idx)
{
printf(";Sensor %u", idx);

++idx;
}

printf(";Position;Position3;Scenario;Speed Left; Speed Right\n");
}

/**
* Log the timestamp as CSV data.
*/
static void logCsvDataTimestamp()
{
printf("%u;", millis());
}

/**
* Log the sensor values and the calculated positions as CSV data.
*
* @param[in] lineSensorValues The array of line sensor values.
* @param[in] length The number of line sensors.
* @param[in] pos The calculated position by all line sensors.
* @param[in] pos3 The calculated position by the inner line sensors.
* @param[in] isPos3Valid Flag to see whether the inner line sensors position is valid or not.
*/
static void logCsvData(const uint16_t* lineSensorValues, uint8_t length, int16_t pos, int16_t pos3, bool isPos3Valid)
{
uint8_t idx = 0;

while (length > idx)
{
if (0 < idx)
{
printf(";");
}

printf("%u", lineSensorValues[idx]);

++idx;
}

printf(";%d;", pos);

if (true == isPos3Valid)
{
printf("%d", pos3);
}
}

/**
* Log the track status as CSV data.
*
* @param[in] trackStatus The track status.
*/
void logCsvDataTrackStatus(DrivingState::TrackStatus trackStatus)
{
switch (trackStatus)
{
case DrivingState::TRACK_STATUS_NORMAL:
printf(";\"Normal\"");
break;

case DrivingState::TRACK_STATUS_START_STOP_LINE:
printf(";\"Start-/Stop-line\"");
break;

case DrivingState::TRACK_STATUS_RIGHT_ANGLE_CURVE_LEFT:
printf(";\"Right angle curve left\"");
break;

case DrivingState::TRACK_STATUS_RIGHT_ANGLE_CURVE_RIGHT:
printf(";\"Right angle curve right\"");
break;

case DrivingState::TRACK_STATUS_SHARP_CURVE_LEFT:
printf(";\"Sharp curve left\"");
break;

case DrivingState::TRACK_STATUS_SHARP_CURVE_RIGHT:
printf(";\"Sharp curve right\"");
break;

case DrivingState::TRACK_STATUS_SHARP_CURVE_LEFT_TURN:
printf(";\"Sharp curve left turn\"");
break;

case DrivingState::TRACK_STATUS_SHARP_CURVE_RIGHT_TURN:
printf(";\"Sharp curve right turn\"");
break;

case DrivingState::TRACK_STATUS_TRACK_LOST_BY_GAP:
printf(";\"Track lost by gap\"");
break;

case DrivingState::TRACK_STATUS_TRACK_LOST_BY_MANOEUVRE:
printf(";\"Track lost by manoeuvre\"");
break;

case DrivingState::TRACK_STATUS_FINISHED:
printf(";\"Track finished\"");
break;

default:
printf(";\"?\"");
break;
}
}

/**
* Log the motor speed set points as CSV data.
*
* @param[in] leftSpeed Right motor speed set point in steps/s.
* @param[in] rightSpeed Left motor speed set point in step/s.
*/
static void logCsvDataSpeed(int16_t leftSpeed, int16_t rightSpeed)
{
printf(";%d;%d", leftSpeed, rightSpeed);
}

#endif /* DEBUG_ALGORITHM */
10 changes: 0 additions & 10 deletions lib/APPSensorFusion/src/DrivingState.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -326,16 +326,6 @@ class DrivingState : public IState
* @return If abort is required, it will return true otherwise false.
*/
bool isAbortRequired();


#ifdef DEBUG_ALGORITHM
/**
* Required for debugging purposes.
*
* @param[in] trackStatus The track status.
*/
friend void logCsvDataTrackStatus(TrackStatus trackStatus);
#endif /* DEBUG_ALGORITHM */
};

/******************************************************************************
Expand Down

0 comments on commit 97dd907

Please sign in to comment.