Skip to content

Commit

Permalink
Merge pull request #146 from BlueAndi/dev/speed_units
Browse files Browse the repository at this point in the history 
Use mm/s for external communication
  • Loading branch information
@gabryelreyes
gabryelreyes authored Aug 16, 2024
2 parents 8aa973f + 9570fe8 commit 87a8f48
Show file tree
Hide file tree
Showing 9 changed files with 88 additions and 44 deletions.
3 changes: 1 addition & 2 deletions lib/APPCalib/src/MotorSpeedCalibrationState.cpp
View file
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
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
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
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
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
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
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
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
39 changes: 33 additions & 6 deletions lib/Service/src/Util.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,7 @@
#include <stdint.h>
#include <stdlib.h>
#include <IButton.h>
#include <RobotConstants.h>

/**
* Utilities
Expand Down Expand Up @@ -85,35 +86,61 @@ 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
*/
inline int32_t stepsPerSecondToMillimetersPerSecond(int16_t speedStepsPerSec)
{
return (static_cast<int32_t>(speedStepsPerSec) * 1000 /
static_cast<int32_t>(RobotConstants::ENCODER_STEPS_PER_M));
}

/**
* 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
*/
inline int16_t millimetersPerSecondToStepsPerSecond(int32_t speedMmPerSec)
{
int32_t speedStepsPerSec = speedMmPerSec * static_cast<int32_t>(RobotConstants::ENCODER_STEPS_PER_M);
return (static_cast<int16_t>(speedStepsPerSec / 1000));
}

} /* namespace Util */

#endif /* UTIL_H */
Expand Down

0 comments on commit 87a8f48

Please sign in to comment.