Skip to content

Commit

Permalink
Applications send a receive speeds in mm/s instead of steps/s
Browse files Browse the repository at this point in the history
  • Loading branch information
@gabryelreyes
gabryelreyes committed Aug 16, 2024
1 parent 436d392 commit 8784e9f
Show file tree
Hide file tree
Showing 6 changed files with 53 additions and 35 deletions.
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
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

0 comments on commit 8784e9f

Please sign in to comment.