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

Use mm/s #153

Merged
merged 5 commits into from
Aug 16, 2024
Merged

Use mm/s #153

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
4a76282
Use mm/s in PlatoonServices
gabryelreyes Aug 16, 2024
441dcc5
Use mm/s in ConvoyLeader
gabryelreyes Aug 16, 2024
314abd7
Use mm/s in RemoteControl
gabryelreyes Aug 16, 2024
5149f8e
Use mm/s in ConvoyFollower
gabryelreyes Aug 16, 2024
e1a9f4c
Adjusted Heading finder P numerator to adjust for speed units
gabryelreyes Aug 16, 2024
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
4 changes: 2 additions & 2 deletions lib/ConvoyFollower/src/App.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -388,8 +388,8 @@ void App::processPeriodicTasks()

if ((true == m_motorSpeedTimer.isTimeout()) && (true == m_smpServer.isSynced()))
{
int16_t leftSpeed = 0;
int16_t rightSpeed = 0;
int32_t leftSpeed = 0;
int32_t rightSpeed = 0;
SpeedData payload;

if (false == DrivingState::getInstance().getMotorSpeedSetpoints(leftSpeed, rightSpeed))
Expand Down
6 changes: 3 additions & 3 deletions lib/ConvoyFollower/src/DrivingState.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -127,7 +127,7 @@ void DrivingState::process(StateMachine& sm)
else
{
int32_t pidDelta = 0;
int16_t centerSpeedSetpoint = 0;
int32_t centerSpeedSetpoint = 0;

/* Longitudinal controller. */
if (true == m_ivsPidProcessTimer.isTimeout())
Expand Down Expand Up @@ -176,12 +176,12 @@ void DrivingState::exit()
m_isActive = false;
}

void DrivingState::setMaxMotorSpeed(int16_t maxSpeed)
void DrivingState::setMaxMotorSpeed(int32_t maxSpeed)
{
m_maxMotorSpeed = maxSpeed;
}

bool DrivingState::getMotorSpeedSetpoints(int16_t& leftMotorSpeed, int16_t& rightMotorSpeed) const
bool DrivingState::getMotorSpeedSetpoints(int32_t& leftMotorSpeed, int32_t& rightMotorSpeed) const
{
leftMotorSpeed = m_leftMotorSpeed;
rightMotorSpeed = m_rightMotorSpeed;
Expand Down
24 changes: 12 additions & 12 deletions lib/ConvoyFollower/src/DrivingState.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -102,19 +102,19 @@ class DrivingState : public IState
/**
* Set maximum motor speed.
*
* @param[in] maxSpeed Maximum motor speed.
* @param[in] maxSpeed Maximum motor speed in mm/s.
*/
void setMaxMotorSpeed(int16_t maxSpeed);
void setMaxMotorSpeed(int32_t maxSpeed);

/**
* Get the calculated motor speed setpoints.
*
* @param[out] leftMotorSpeed The calculated left motor speed.
* @param[out] rightMotorSpeed The calculated right motor speed.
* @param[out] leftMotorSpeed The calculated left motor speed in mm/s.
* @param[out] rightMotorSpeed The calculated right motor speed in mm/s.
*
* @returns true if the speed setpoints are valid. Otherwise, false.
*/
bool getMotorSpeedSetpoints(int16_t& leftMotorSpeed, int16_t& rightMotorSpeed) const;
bool getMotorSpeedSetpoints(int32_t& leftMotorSpeed, int32_t& rightMotorSpeed) const;

/**
* Set latest vehicle data.
Expand Down Expand Up @@ -214,7 +214,7 @@ class DrivingState : public IState
struct HEADING_FINDER_PID_FACTORS
{
/** The PID proportional factor numerator for the Heading Finder. */
static const int32_t PID_P_NUMERATOR = 5;
static const int32_t PID_P_NUMERATOR = 1;

/** The PID proportional factor denominator for the Heading Finder.*/
static const int32_t PID_P_DENOMINATOR = 4;
Expand All @@ -235,14 +235,14 @@ class DrivingState : public IState
/** Flag: State is active. */
bool m_isActive;

/** Maximum motor speed. */
int16_t m_maxMotorSpeed;
/** Maximum motor speed in mm/s. */
int32_t m_maxMotorSpeed;

/** Calculated left motor speed. */
int16_t m_leftMotorSpeed;
/** Calculated left motor speed in mm/s. */
int32_t m_leftMotorSpeed;

/** Calculated left motor speed. */
int16_t m_rightMotorSpeed;
/** Calculated left motor speed in mm/s. */
int32_t m_rightMotorSpeed;

/** Latest vehicle data. */
Telemetry m_vehicleData;
Expand Down
14 changes: 7 additions & 7 deletions lib/ConvoyFollower/src/SerialMuxChannels.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -167,16 +167,16 @@ typedef struct _CommandResponse
/** Response Payload. */
union
{
int16_t maxMotorSpeed; /**< Max speed [steps/s]. */
int32_t maxMotorSpeed; /**< Max speed [mm/s]. */
};
} __attribute__((packed)) CommandResponse;

/** Struct of the "Speed" channel payload. */
typedef struct _SpeedData
{
int16_t left; /**< Left motor speed [steps/s] */
int16_t right; /**< Right motor speed [steps/s] */
int16_t center; /**< Center motor speed [steps/s] */
int32_t left; /**< Left motor speed [mm/s] */
int32_t right; /**< Right motor speed [mm/s] */
int32_t center; /**< Center motor speed [mm/s] */
} __attribute__((packed)) SpeedData;

/** Struct of the "Current Vehicle Data" channel payload. */
Expand All @@ -185,9 +185,9 @@ typedef struct _VehicleData
int32_t xPos; /**< X position [mm]. */
int32_t yPos; /**< Y position [mm]. */
int32_t orientation; /**< Orientation [mrad]. */
int16_t left; /**< Left motor speed [steps/s]. */
int16_t right; /**< Right motor speed [steps/s]. */
int16_t center; /**< Center speed [steps/s]. */
int32_t left; /**< Left motor speed [mm/s]. */
int32_t right; /**< Right motor speed [mm/s]. */
int32_t center; /**< Center speed [mm/s]. */
SMPChannelPayload::Range proximity; /**< Range at which object is found [range]. */
} __attribute__((packed)) VehicleData;

Expand Down
2 changes: 1 addition & 1 deletion lib/ConvoyLeader/src/App.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -382,7 +382,7 @@ void App::processPeriodicTasks()

if ((true == m_motorSpeedTimer.isTimeout()) && (true == m_smpServer.isSynced()))
{
int16_t centerSpeed = 0;
int32_t centerSpeed = 0;
SpeedData payload;

if (false == DrivingState::getInstance().getTopMotorSpeed(centerSpeed))
Expand Down
12 changes: 6 additions & 6 deletions lib/ConvoyLeader/src/DrivingState.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,8 +79,8 @@ void DrivingState::process(StateMachine& sm)
/* Compact implementation of the "Platoon Application Controller". */

/* Drive as fast as possible */
int16_t linearSpeedSetpoint = m_maxMotorSpeed;
int16_t controlSpeed = m_maxMotorSpeed;
int32_t linearSpeedSetpoint = m_maxMotorSpeed;
int32_t controlSpeed = m_maxMotorSpeed;

/* Constrain the setpoint based on platoon length. */
platoonLengthController(linearSpeedSetpoint);
Expand Down Expand Up @@ -115,13 +115,13 @@ void DrivingState::exit()
m_isActive = false;
}

void DrivingState::setMaxMotorSpeed(int16_t maxSpeed)
void DrivingState::setMaxMotorSpeed(int32_t maxSpeed)
{
m_maxMotorSpeed = maxSpeed;
LOG_DEBUG("DrivingState: Max motor speed: %d", m_maxMotorSpeed);
}

bool DrivingState::getTopMotorSpeed(int16_t& topMotorSpeed) const
bool DrivingState::getTopMotorSpeed(int32_t& topMotorSpeed) const
{
topMotorSpeed = m_currentSpeedSetpoint;

Expand Down Expand Up @@ -151,12 +151,12 @@ void DrivingState::setPlatoonLength(const int32_t platoonLength)
* Private Methods
*****************************************************************************/

void DrivingState::platoonLengthController(int16_t& linearCenterSpeedSetpoint)
void DrivingState::platoonLengthController(int32_t& linearCenterSpeedSetpoint)
{
/* Limit the speed setpoint if platoon length greater than maximum allowed. */
if (m_platoonLength > MAX_PLATOON_LENGTH)
{
int16_t maxPossibleSpeed = linearCenterSpeedSetpoint;
int32_t maxPossibleSpeed = linearCenterSpeedSetpoint;

maxPossibleSpeed = maxPossibleSpeed - (m_platoonLength * maxPossibleSpeed / (MAX_PLATOON_LENGTH * 10));

Expand Down
18 changes: 9 additions & 9 deletions lib/ConvoyLeader/src/DrivingState.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -96,18 +96,18 @@ class DrivingState : public IState
* Set maximum motor speed.
* This also sets the range factor as is dependent on the maximum motor speed.
*
* @param[in] maxSpeed Maximum motor speed.
* @param[in] maxSpeed Maximum motor speed in mm/s.
*/
void setMaxMotorSpeed(int16_t maxSpeed);
void setMaxMotorSpeed(int32_t maxSpeed);

/**
* Get the calculated top motor speed.
*
* @param[out] topMotorSpeed The calculated top motor speed.
* @param[out] topMotorSpeed The calculated top motor speed in mm/s.
*
* @returns true if the top motor speed is valid. Otherwise, false.
*/
bool getTopMotorSpeed(int16_t& topMotorSpeed) const;
bool getTopMotorSpeed(int32_t& topMotorSpeed) const;

/**
* Set latest vehicle data.
Expand Down Expand Up @@ -147,11 +147,11 @@ class DrivingState : public IState
/** Flag: State is active. */
bool m_isActive;

/** Maximum motor speed in steps/s. */
int16_t m_maxMotorSpeed;
/** Maximum motor speed in mm/s. */
int32_t m_maxMotorSpeed;

/** Current linear speed setpoint to apply to the vehicle. */
int16_t m_currentSpeedSetpoint;
/** Current linear speed setpoint to apply to the vehicle in mm/s. */
int32_t m_currentSpeedSetpoint;

/** Latest vehicle data. */
Telemetry m_vehicleData;
Expand All @@ -167,7 +167,7 @@ class DrivingState : public IState
*
* @param[out] speedSetpoint The speed setpoint.
*/
void platoonLengthController(int16_t& speedSetpoint);
void platoonLengthController(int32_t& speedSetpoint);

/**
* Default constructor.
Expand Down
14 changes: 7 additions & 7 deletions lib/ConvoyLeader/src/SerialMuxChannels.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -167,16 +167,16 @@ typedef struct _CommandResponse
/** Response Payload. */
union
{
int16_t maxMotorSpeed; /**< Max speed [steps/s]. */
int32_t maxMotorSpeed; /**< Max speed [mm/s]. */
};
} __attribute__((packed)) CommandResponse;

/** Struct of the "Speed" channel payload. */
typedef struct _SpeedData
{
int16_t left; /**< Left motor speed [steps/s] */
int16_t right; /**< Right motor speed [steps/s] */
int16_t center; /**< Center motor speed [steps/s] */
int32_t left; /**< Left motor speed [mm/s] */
int32_t right; /**< Right motor speed [mm/s] */
int32_t center; /**< Center motor speed [mm/s] */
} __attribute__((packed)) SpeedData;

/** Struct of the "Current Vehicle Data" channel payload. */
Expand All @@ -185,9 +185,9 @@ typedef struct _VehicleData
int32_t xPos; /**< X position [mm]. */
int32_t yPos; /**< Y position [mm]. */
int32_t orientation; /**< Orientation [mrad]. */
int16_t left; /**< Left motor speed [steps/s]. */
int16_t right; /**< Right motor speed [steps/s]. */
int16_t center; /**< Center speed [steps/s]. */
int32_t left; /**< Left motor speed [mm/s]. */
int32_t right; /**< Right motor speed [mm/s]. */
int32_t center; /**< Center speed [mm/s]. */
SMPChannelPayload::Range proximity; /**< Range at which object is found [range]. */
} __attribute__((packed)) VehicleData;

Expand Down
12 changes: 6 additions & 6 deletions lib/PlatoonService/src/CollisionAvoidance.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ class CollisionAvoidance
*/
CollisionAvoidance(Telemetry::Range closestProximityRangeValue, Telemetry::Range rangeThreshold) :
m_closestProximityRangeValue(closestProximityRangeValue),
m_rangeThreshold(rangeThreshold){};
m_rangeThreshold(rangeThreshold) {};

/**
* Default Destructor.
Expand All @@ -80,10 +80,10 @@ class CollisionAvoidance
/**
* Limit the speed of the vehicle to avoid collision.
*
* @param[out] speedSetpoint The speed setpoint to be limited in steps/s.
* @param[out] speedSetpoint The speed setpoint to be limited in mm/s.
* @param[in] vehicleData The vehicle data structure.
*/
void limitSpeedToAvoidCollision(int16_t& speedSetpoint, const Telemetry& vehicleData) const
void limitSpeedToAvoidCollision(int32_t& speedSetpoint, const Telemetry& vehicleData) const
{
/* Is vehicle is closer than the threshold? */
if (vehicleData.proximity >= m_rangeThreshold)
Expand All @@ -96,11 +96,11 @@ class CollisionAvoidance
/**
* Limit the speed of the vehicle to avoid collision.
*
* @param[out] leftSpeedSetpoint The left motor speed setpoint to be limited in steps/s.
* @param[out] rightSpeedSetpoint The right motor speed setpoint to be limited in steps/s.
* @param[out] leftSpeedSetpoint The left motor speed setpoint to be limited in mm/s.
* @param[out] rightSpeedSetpoint The right motor speed setpoint to be limited in mm/s.
* @param[in] vehicleData The vehicle data structure.
*/
void limitSpeedToAvoidCollision(int16_t& leftSpeedSetpoint, int16_t& rightSpeedSetpoint,
void limitSpeedToAvoidCollision(int32_t& leftSpeedSetpoint, int32_t& rightSpeedSetpoint,
const Telemetry& vehicleData) const
{
/* Is vehicle is closer than the threshold? */
Expand Down
6 changes: 3 additions & 3 deletions lib/PlatoonService/src/HeadingFinder.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -93,9 +93,9 @@ void HeadingFinder::setPIDFactors(int32_t pNumerator, int32_t pDenominator, int3
m_pidCtrl.setDFactor(dNumerator, dDenominator);
}

int16_t HeadingFinder::process(int16_t& targetSpeedLeft, int16_t& targetSpeedRight)
int32_t HeadingFinder::process(int32_t& targetSpeedLeft, int32_t& targetSpeedRight)
{
int16_t pidDelta = 0;
int32_t pidDelta = 0;

/* Process PID controller when timer is done, new data is found, and the motors have a setpoint greater than 0. */
if ((true == m_pidProcessTime.isTimeout()) && (true == m_newOdometryData) &&
Expand Down Expand Up @@ -152,7 +152,7 @@ void HeadingFinder::setOdometryData(int32_t xPos, int32_t yPos, int32_t heading)
m_newOdometryData = true;
}

void HeadingFinder::setMotorSpeedData(int16_t speedLeft, int16_t speedRight)
void HeadingFinder::setMotorSpeedData(int32_t speedLeft, int32_t speedRight)
{
/* Set motor speeds. */
m_data.currentSpeedLeft = speedLeft;
Expand Down
22 changes: 11 additions & 11 deletions lib/PlatoonService/src/HeadingFinder.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -70,8 +70,8 @@ class HeadingFinder
int32_t currentXPos; /**< Current X position [mm]. */
int32_t currentYPos; /**< Current Y position[mm]. */
int32_t currentHeading; /**< Current heading [mrad]. */
int16_t currentSpeedLeft; /**< Current speed of the left motor [steps/s]. */
int16_t currentSpeedRight; /**< Current speed of the right motor [steps/s]. */
int32_t currentSpeedLeft; /**< Current speed of the left motor [mm/s]. */
int32_t currentSpeedRight; /**< Current speed of the right motor [mm/s]. */
int32_t targetXPos; /**< Target X position[mm]. */
int32_t targetYPos; /**< Target Y position[mm]. */
int32_t targetHeading; /**< Target heading [mrad]. */
Expand All @@ -98,13 +98,13 @@ class HeadingFinder

/**
* Process the Heading finder and calculate the target speed for the motors.
* @param[out] targetSpeedLeft Target speed of the left motor [steps/s].
* @param[out] targetSpeedRight Target speed of the right motor [steps/s].
* @param[out] targetSpeedLeft Target speed of the left motor [mm/s].
* @param[out] targetSpeedRight Target speed of the right motor [mm/s].
*
* @return the delta speed [steps/s] calculated by the PID controller, if available.
* @return the delta speed [mm/s] calculated by the PID controller, if available.
* Otherwise 0.
*/
int16_t process(int16_t& targetSpeedLeft, int16_t& targetSpeedRight);
int32_t process(int32_t& targetSpeedLeft, int32_t& targetSpeedRight);

/**
* Set lastest odometry data.
Expand All @@ -116,10 +116,10 @@ class HeadingFinder

/**
* Set latest motor speed data.
* @param[in] speedLeft Speed of the left motor [steps/s].
* @param[in] speedRight Speed of the right motor [steps/s].
* @param[in] speedLeft Speed of the left motor [mm/s].
* @param[in] speedRight Speed of the right motor [mm/s].
*/
void setMotorSpeedData(int16_t speedLeft, int16_t speedRight);
void setMotorSpeedData(int32_t speedLeft, int32_t speedRight);

/**
* Set the target heading the robot shall face to using its x and y coordinates.
Expand All @@ -139,8 +139,8 @@ class HeadingFinder
/** Period in ms for PID processing. */
static const uint32_t PID_PROCESS_PERIOD = 40U;

/** Maximum motor speed in encoder steps/s */
static const int16_t MAX_MOTOR_SPEED = 3000;
/** Maximum motor speed in mm/s */
static const int32_t MAX_MOTOR_SPEED = 373;

/** The PID proportional factor numerator for the heading controller. */
static const int32_t PID_P_NUMERATOR = 4;
Expand Down
14 changes: 7 additions & 7 deletions lib/PlatoonService/src/Telemetry.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,9 +79,9 @@ struct Telemetry
int32_t xPos; /**< X position [mm]. */
int32_t yPos; /**< Y position [mm]. */
int32_t orientation; /**< Orientation [mrad]. */
int16_t left; /**< Left motor speed [steps/s]. */
int16_t right; /**< Right motor speed [steps/s]. */
int16_t center; /**< Center speed [steps/s]. */
int32_t left; /**< Left motor speed [mm/s]. */
int32_t right; /**< Right motor speed [mm/s]. */
int32_t center; /**< Center speed [mm/s]. */
Telemetry::Range proximity; /**< Range at which object is found [range]. */

/**
Expand All @@ -97,12 +97,12 @@ struct Telemetry
* @param[in] xPos X position [mm].
* @param[in] yPos Y position [mm].
* @param[in] orientation Orientation [mrad].
* @param[in] left Left motor speed [steps/s].
* @param[in] right Right motor speed [steps/s].
* @param[in] center Center speed [steps/s].
* @param[in] left Left motor speed [mm/s].
* @param[in] right Right motor speed [mm/s].
* @param[in] center Center speed [mm/s].
* @param[in] proximity Range at which object is found [range].
*/
Telemetry(int32_t xPos, int32_t yPos, int32_t orientation, int16_t left, int16_t right, int16_t center,
Telemetry(int32_t xPos, int32_t yPos, int32_t orientation, int32_t left, int32_t right, int32_t center,
Telemetry::Range proximity) :
xPos(xPos),
yPos(yPos),
Expand Down
Loading