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 for external communication #146

Merged
merged 3 commits into from
Aug 16, 2024
Merged

Use mm/s for external communication #146

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
436d392
Added util to convert encoders and millimeters per second
gabryelreyes Aug 16, 2024
8784e9f
Applications send a receive speeds in mm/s instead of steps/s
gabryelreyes Aug 16, 2024
9570fe8
Moved utils from source to header file to aid compiler optimization
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
3 changes: 1 addition & 2 deletions lib/APPCalib/src/MotorSpeedCalibrationState.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -227,8 +227,7 @@ void MotorSpeedCalibrationState::finishCalibration(StateMachine& sm)
}
else
{
int32_t maxSpeed32 =
static_cast<int32_t>(maxSpeed) * 1000 / static_cast<int32_t>(RobotConstants::ENCODER_STEPS_PER_M);
int32_t maxSpeed32 = Util::stepsPerSecondToMillimetersPerSecond(maxSpeed);

LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed);
LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed32);
Expand Down
17 changes: 12 additions & 5 deletions lib/APPConvoyFollower/src/App.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
#include <DifferentialDrive.h>
#include <Odometry.h>
#include <Logging.h>
#include <Util.h>

/******************************************************************************
* Compiler Switches
Expand Down Expand Up @@ -189,7 +190,8 @@ void App::handleRemoteCommand(const Command& cmd)
break;

case SMPChannelPayload::CmdId::CMD_ID_GET_MAX_SPEED:
rsp.maxMotorSpeed = Board::getInstance().getSettings().getMaxSpeed();
rsp.maxMotorSpeed =
Util::stepsPerSecondToMillimetersPerSecond(Board::getInstance().getSettings().getMaxSpeed());
break;

case SMPChannelPayload::CmdId::CMD_ID_SET_INIT_POS:
Expand Down Expand Up @@ -252,6 +254,9 @@ void App::reportVehicleData()
uint8_t averageCounts = 0U;
uint8_t leftCounts = 0U;
uint8_t rightCounts = 0U;
int16_t leftSpeed = speedometer.getLinearSpeedLeft();
int16_t rightSpeed = speedometer.getLinearSpeedRight();
int16_t centerSpeed = speedometer.getLinearSpeedCenter();

proximitySensors.read();
leftCounts = proximitySensors.countsFrontWithLeftLeds();
Expand All @@ -265,9 +270,9 @@ void App::reportVehicleData()
payload.xPos = xPos;
payload.yPos = yPos;
payload.orientation = odometry.getOrientation();
payload.left = speedometer.getLinearSpeedLeft();
payload.right = speedometer.getLinearSpeedRight();
payload.center = speedometer.getLinearSpeedCenter();
payload.left = Util::stepsPerSecondToMillimetersPerSecond(leftSpeed);
payload.right = Util::stepsPerSecondToMillimetersPerSecond(rightSpeed);
payload.center = Util::stepsPerSecondToMillimetersPerSecond(centerSpeed);
payload.proximity = static_cast<SMPChannelPayload::Range>(averageCounts);

/* Ignoring return value, as error handling is not available. */
Expand Down Expand Up @@ -359,7 +364,9 @@ void App_motorSpeedSetpointsChannelCallback(const uint8_t* payload, const uint8_
if ((nullptr != payload) && (SPEED_SETPOINT_CHANNEL_DLC == payloadSize))
{
const SpeedData* motorSpeedData = reinterpret_cast<const SpeedData*>(payload);
DrivingState::getInstance().setTargetSpeeds(motorSpeedData->left, motorSpeedData->right);
int16_t leftSpeed = Util::millimetersPerSecondToStepsPerSecond(motorSpeedData->left);
int16_t rightSpeed = Util::millimetersPerSecondToStepsPerSecond(motorSpeedData->right);
DrivingState::getInstance().setTargetSpeeds(leftSpeed, rightSpeed);
}
}

Expand Down
14 changes: 7 additions & 7 deletions lib/APPConvoyFollower/src/SerialMuxChannels.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -169,16 +169,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 @@ -187,9 +187,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
16 changes: 11 additions & 5 deletions lib/APPConvoyLeader/src/App.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
#include <DifferentialDrive.h>
#include <Odometry.h>
#include <Logging.h>
#include <Util.h>

/******************************************************************************
* Compiler Switches
Expand Down Expand Up @@ -182,7 +183,8 @@ void App::handleRemoteCommand(const Command& cmd)
switch (cmd.commandId)
{
case SMPChannelPayload::CmdId::CMD_ID_GET_MAX_SPEED:
rsp.maxMotorSpeed = ParameterSets::getInstance().getParameterSet().topSpeed;
rsp.maxMotorSpeed =
Util::stepsPerSecondToMillimetersPerSecond(ParameterSets::getInstance().getParameterSet().topSpeed);
break;

case SMPChannelPayload::CmdId::CMD_ID_SET_INIT_POS:
Expand Down Expand Up @@ -245,6 +247,9 @@ void App::reportVehicleData()
uint8_t averageCounts = 0U;
uint8_t leftCounts = 0U;
uint8_t rightCounts = 0U;
int16_t leftSpeed = speedometer.getLinearSpeedLeft();
int16_t rightSpeed = speedometer.getLinearSpeedRight();
int16_t centerSpeed = speedometer.getLinearSpeedCenter();

proximitySensors.read();
leftCounts = proximitySensors.countsFrontWithLeftLeds();
Expand All @@ -258,9 +263,9 @@ void App::reportVehicleData()
payload.xPos = xPos;
payload.yPos = yPos;
payload.orientation = odometry.getOrientation();
payload.left = speedometer.getLinearSpeedLeft();
payload.right = speedometer.getLinearSpeedRight();
payload.center = speedometer.getLinearSpeedCenter();
payload.left = Util::stepsPerSecondToMillimetersPerSecond(leftSpeed);
payload.right = Util::stepsPerSecondToMillimetersPerSecond(rightSpeed);
payload.center = Util::stepsPerSecondToMillimetersPerSecond(centerSpeed);
payload.proximity = static_cast<SMPChannelPayload::Range>(averageCounts);

/* Ignoring return value, as error handling is not available. */
Expand Down Expand Up @@ -331,7 +336,8 @@ void App_motorSpeedSetpointsChannelCallback(const uint8_t* payload, const uint8_
if ((nullptr != payload) && (SPEED_SETPOINT_CHANNEL_DLC == payloadSize))
{
const SpeedData* motorSpeedData = reinterpret_cast<const SpeedData*>(payload);
DrivingState::getInstance().setTopSpeed(motorSpeedData->center);
int16_t topSpeed = Util::millimetersPerSecondToStepsPerSecond(motorSpeedData->center);
DrivingState::getInstance().setTopSpeed(topSpeed);
}
}

Expand Down
14 changes: 7 additions & 7 deletions lib/APPConvoyLeader/src/SerialMuxChannels.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -163,16 +163,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 @@ -181,9 +181,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/APPLineFollower/src/MotorSpeedCalibrationState.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -227,7 +227,7 @@ void MotorSpeedCalibrationState::finishCalibration(StateMachine& sm)
}
else
{
int32_t maxSpeed32 = static_cast<int32_t>(maxSpeed) * 1000 / static_cast<int32_t>(RobotConstants::ENCODER_STEPS_PER_M);
int32_t maxSpeed32 = Util::stepsPerSecondToMillimetersPerSecond(maxSpeed);

LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed);
LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed32);
Expand Down
13 changes: 9 additions & 4 deletions lib/APPRemoteControl/src/App.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
#include <DifferentialDrive.h>
#include <Odometry.h>
#include <Logging.h>
#include <Util.h>

/******************************************************************************
* Compiler Switches
Expand Down Expand Up @@ -189,7 +190,8 @@ void App::handleRemoteCommand(const Command& cmd)
break;

case SMPChannelPayload::CmdId::CMD_ID_GET_MAX_SPEED:
rsp.maxMotorSpeed = Board::getInstance().getSettings().getMaxSpeed();
rsp.maxMotorSpeed =
Util::stepsPerSecondToMillimetersPerSecond(Board::getInstance().getSettings().getMaxSpeed());
break;

case SMPChannelPayload::CmdId::CMD_ID_SET_INIT_POS:
Expand Down Expand Up @@ -252,6 +254,9 @@ void App::reportVehicleData()
uint8_t averageCounts = 0U;
uint8_t leftCounts = 0U;
uint8_t rightCounts = 0U;
int16_t leftSpeed = speedometer.getLinearSpeedLeft();
int16_t rightSpeed = speedometer.getLinearSpeedRight();
int16_t centerSpeed = speedometer.getLinearSpeedCenter();

proximitySensors.read();
leftCounts = proximitySensors.countsFrontWithLeftLeds();
Expand All @@ -265,9 +270,9 @@ void App::reportVehicleData()
payload.xPos = xPos;
payload.yPos = yPos;
payload.orientation = odometry.getOrientation();
payload.left = speedometer.getLinearSpeedLeft();
payload.right = speedometer.getLinearSpeedRight();
payload.center = speedometer.getLinearSpeedCenter();
payload.left = Util::stepsPerSecondToMillimetersPerSecond(leftSpeed);
payload.right = Util::stepsPerSecondToMillimetersPerSecond(rightSpeed);
payload.center = Util::stepsPerSecondToMillimetersPerSecond(centerSpeed);
payload.proximity = static_cast<SMPChannelPayload::Range>(averageCounts);

/* Ignoring return value, as error handling is not available. */
Expand Down
14 changes: 7 additions & 7 deletions lib/APPRemoteControl/src/SerialMuxChannels.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -169,16 +169,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 @@ -187,9 +187,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: 12 additions & 0 deletions lib/Service/src/Util.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@
* Includes
*****************************************************************************/
#include <Util.h>
#include <RobotConstants.h>

/******************************************************************************
* Compiler Switches
Expand Down Expand Up @@ -210,6 +211,17 @@ bool Util::isButtonTriggered(IButton& button, bool& lastState)
return isTriggered;
}

int32_t Util::stepsPerSecondToMillimetersPerSecond(int16_t speedStepsPerSec)
{
return (static_cast<int32_t>(speedStepsPerSec) * 1000 / static_cast<int32_t>(RobotConstants::ENCODER_STEPS_PER_M));
BlueAndi marked this conversation as resolved.
Show resolved Hide resolved
}

int16_t Util::millimetersPerSecondToStepsPerSecond(int32_t speedMmPerSec)
{
int32_t speedStepsPerSec = speedMmPerSec * static_cast<int32_t>(RobotConstants::ENCODER_STEPS_PER_M);
return (static_cast<int16_t>(speedStepsPerSec / 1000));
}

/******************************************************************************
* Local Functions
*****************************************************************************/
30 changes: 24 additions & 6 deletions lib/Service/src/Util.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,35 +85,53 @@ namespace Util

/**
* Divide and round.
*
*
* @param[in] numerator The numerator.
* @param[in] denominator The denominator.
*
*
* @return Result
*/
uint32_t divRoundUp(uint32_t numerator, uint32_t denominator);

/**
* Divide and round.
*
*
* @param[in] numerator The numerator.
* @param[in] denominator The denominator.
*
*
* @return Result
*/
int32_t divRoundUp(int32_t numerator, int32_t denominator);

/**
* Is button triggered?
* Triggered means a pressed/released change.
*
*
* @param[in] button The button.
* @param[inout] lastState The last button state.
*
*
* @return If button is triggered, it will return true otherwise false.
*/
bool isButtonTriggered(IButton& button, bool& lastState);

/**
* Convert a speed in encoder steps per second to a speed in mm/s.
*
* @param[in] speedStepsPerSec Speed in encoder steps per second
*
* @return Speed in mm/s
*/
int32_t stepsPerSecondToMillimetersPerSecond(int16_t speedStepsPerSec);

/**
* Convert a speed in mm/s to a speed in encoder steps per second.
*
* @param[in] speedMmPerSec Speed in mm/s
*
* @return Speed in encoder steps per second
*/
int16_t millimetersPerSecondToStepsPerSecond(int32_t speedMmPerSec);
gabryelreyes marked this conversation as resolved.
Show resolved Hide resolved

} /* namespace Util */

#endif /* UTIL_H */
Expand Down