void RoboClaw::printStatus(char *string, size_t n)
{
snprintf(string, n,
"pos1,spd1,pos2,spd2: %10.2f %10.2f %10.2f %10.2f\n",
double(getMotorPosition(MOTOR_1)),
double(getMotorSpeed(MOTOR_1)),
double(getMotorPosition(MOTOR_2)),
double(getMotorSpeed(MOTOR_2)));
}
void motion_SpeedControlLR(RobotCommand *out_cmd, float spLeft, float distLeft,
float accLeft, float spRight, float distRight, float accRight) {
out_cmd->cmdType = SPEED_COMMAND;
out_cmd->mcType = LEFT_RIGHT;
//LEFT
ComputeSpeedCommand(&(out_cmd->cmd.speedCmd[LEFT_MOTOR]),
getMotorSpeed(&motors[LEFT_RIGHT][LEFT_MOTOR]),
convertSpeedTovTopsPerPeriod(spLeft), convertDistTovTops(distLeft),
convertAccelTovTopsPerPeriodSqd(accLeft));
//RIGHT
ComputeSpeedCommand(&(out_cmd->cmd.speedCmd[RIGHT_MOTOR]),
getMotorSpeed(&motors[LEFT_RIGHT][RIGHT_MOTOR]),
convertSpeedTovTopsPerPeriod(spRight),
convertDistTovTops(distRight),
convertAccelTovTopsPerPeriodSqd(accRight));
}
void motion_SpeedControlADMaxTime(RobotCommand *out_cmd, float spAlpha,
float accAlpha, float spDelta, float accDelta, int duringMs) {
int32 time;
out_cmd->cmdType = SPEED_COMMAND;
out_cmd->mcType = ALPHA_DELTA;
time = (DEFAULT_SAMPLING_FREQ * duringMs) / 1000;
ComputeSpeedCommandMaxTime(&(out_cmd->cmd.speedCmd[ALPHA_MOTOR]),
getMotorSpeed(&motors[ALPHA_DELTA][ALPHA_MOTOR]),
convertSpeedTovTopsPerPeriod(spAlpha),
convertAccelTovTopsPerPeriodSqd(accAlpha), time);
ComputeSpeedCommandMaxTime(&(out_cmd->cmd.speedCmd[DELTA_MOTOR]),
getMotorSpeed(&motors[ALPHA_DELTA][DELTA_MOTOR]),
convertSpeedTovTopsPerPeriod(spDelta),
convertAccelTovTopsPerPeriodSqd(accDelta), time);
}
void motion_SpeedControlAD(RobotCommand *out_cmd, float spAlpha,
float distAlpha, float accAlpha, float spDelta, float distDelta,
float accDelta) {
out_cmd->cmdType = SPEED_COMMAND;
out_cmd->mcType = ALPHA_DELTA;
//ALPHA
ComputeSpeedCommand(&(out_cmd->cmd.speedCmd[ALPHA_MOTOR]),
getMotorSpeed(&motors[ALPHA_DELTA][ALPHA_MOTOR]),
convertSpeedTovTopsPerPeriod(spAlpha),
convertDistTovTops(distAlpha),
convertAccelTovTopsPerPeriodSqd(accAlpha));
//DELTA
ComputeSpeedCommand(&(out_cmd->cmd.speedCmd[DELTA_MOTOR]),
getMotorSpeed(&motors[ALPHA_DELTA][DELTA_MOTOR]),
convertSpeedTovTopsPerPeriod(spDelta),
convertDistTovTops(distDelta),
convertAccelTovTopsPerPeriodSqd(accDelta));
}
void motion_SpeedControlLRDecel(RobotCommand *out_cmd, float spLeft,
float distLeft, float accLeft, float decLeft, float spRight,
float distRight, float accRight, float decRight) {
int32 motorSpeed;
int32 goalSpeed;
out_cmd->cmdType = SPEED_COMMAND;
out_cmd->mcType = LEFT_RIGHT;
motorSpeed = getMotorSpeed(&motors[LEFT_RIGHT][LEFT_MOTOR]);
goalSpeed = convertSpeedTovTopsPerPeriod(spLeft);
if (((motorSpeed < goalSpeed) && motorSpeed > 0)
|| ((motorSpeed > goalSpeed) && motorSpeed < 0)) {
//LEFT
ComputeSpeedCommand(&(out_cmd->cmd.speedCmd[LEFT_MOTOR]), motorSpeed,
goalSpeed, convertDistTovTops(distLeft),
convertAccelTovTopsPerPeriodSqd(accLeft));
} else {
//LEFT
ComputeSpeedCommand(&(out_cmd->cmd.speedCmd[LEFT_MOTOR]), motorSpeed,
goalSpeed, convertDistTovTops(distLeft),
convertAccelTovTopsPerPeriodSqd(decLeft));
}
motorSpeed = getMotorSpeed(&motors[LEFT_RIGHT][RIGHT_MOTOR]);
goalSpeed = convertSpeedTovTopsPerPeriod(spRight);
if (((motorSpeed < goalSpeed) && motorSpeed > 0)
|| ((motorSpeed > goalSpeed) && motorSpeed < 0)) {
//RIGHT
ComputeSpeedCommand(&(out_cmd->cmd.speedCmd[RIGHT_MOTOR]), motorSpeed,
goalSpeed, convertDistTovTops(distRight),
convertAccelTovTopsPerPeriodSqd(accRight));
} else {
//RIGHT
ComputeSpeedCommand(&(out_cmd->cmd.speedCmd[RIGHT_MOTOR]), motorSpeed,
goalSpeed, convertDistTovTops(distRight),
convertAccelTovTopsPerPeriodSqd(decRight));
}
}
bool ChassicCommandInterpreter::handle(const Command& command)
{
if(command.compareModule("CH"))
{
// CHASSIC
if(command.compareAction("MV"))
{
// MOVE
if(command.compareArgument1("FW"))
{
// FORWARD
_platform.move(rover::chassic::MOVE_FORWARD, getMotorSpeed(command, rover::chassic::SPEED_75));
return true;
}
if(command.compareArgument1("BW"))
{
// FORWARD
_platform.move(rover::chassic::MOVE_BACKWARD, getMotorSpeed(command, rover::chassic::SPEED_75));
return true;
}
rover::telemetry::Logger::debug("Wrong CHASSIC MOVE command");
}
else if(command.compareAction("RT"))
{
// ROTATE
if(command.compareArgument1("TL"))
{
// LEFT
_platform.rotate(rover::chassic::ROTATE_LEFT, getMotorSpeed(command, rover::chassic::SPEED_75));
return true;
}
if(command.compareArgument1("TR"))
{
// FORWARD
_platform.rotate(rover::chassic::ROTATE_RIGHT, getMotorSpeed(command, rover::chassic::SPEED_75));
return true;
}
rover::telemetry::Logger::debug("Wrong CHASSIC ROTATE command");
}
else if(command.compareAction("ST"))
{
// STOP
_platform.stop();
return true;
}
else if(command.compareAction("BR"))
{
// BRAKE
_platform.brake();
return true;
}
else
{
rover::telemetry::Logger::debug("Wrong CHASSIC command");
}
return false;
}
}
