diff --git a/src/control/SwerveController.cpp b/src/control/SwerveController.cpp index c81fcf7f..b2b650d7 100644 --- a/src/control/SwerveController.cpp +++ b/src/control/SwerveController.cpp @@ -25,12 +25,16 @@ drive_commands_t SwerveController::setTurnInPlaceCmdVel(double dtheta, return {0.0, 0.0, 0.0, 0.0}; } + // robotVelToWheelVel() will return steers and vels such that the vels are positive. Since + // we're using preset steer angles this isn't correct, since we need to reverse the wheels + // to turn the other way instead of rotating the modules by 180deg. So, we just reverse the + // vels if the target rotVel is negative. kinematics::swervewheelvel_t wheelVels = - swerve_kinematics.robotVelToWheelVel(0, 0, dtheta); - double lfPower = wheelVels.lfVel / Constants::MAX_WHEEL_VEL; - double lbPower = wheelVels.lbVel / Constants::MAX_WHEEL_VEL; - double rfPower = wheelVels.rfVel / Constants::MAX_WHEEL_VEL; - double rbPower = wheelVels.rbVel / Constants::MAX_WHEEL_VEL; + swerve_kinematics.robotVelToWheelVel(0, 0, std::abs(dtheta)); + double lfPower = std::copysign(wheelVels.lfVel, dtheta) / Constants::MAX_WHEEL_VEL; + double lbPower = std::copysign(wheelVels.lbVel, dtheta) / Constants::MAX_WHEEL_VEL; + double rfPower = std::copysign(wheelVels.rfVel, dtheta) / Constants::MAX_WHEEL_VEL; + double rbPower = std::copysign(wheelVels.rbVel, dtheta) / Constants::MAX_WHEEL_VEL; double maxAbsPWM = std::max(std::max(std::abs(lfPower), std::abs(lbPower)), std::max(std::abs(rfPower), std::abs(rbPower))); @@ -105,10 +109,11 @@ swerve_rots_t SwerveController::getSteerRots(DriveMode mode) const { case DriveMode::TurnInPlace: { kinematics::swervewheelvel_t wheelVels = swerve_kinematics.robotVelToWheelVel(0, 0, 1); - return {static_cast(wheelVels.lfRot * 1000), - static_cast(wheelVels.rfRot * 1000), - static_cast(wheelVels.lbRot * 1000), - static_cast(wheelVels.rbRot * 1000)}; + // convert from rad to deg + return {static_cast(wheelVels.lfRot * 1000 * 180 / M_PI), + static_cast(wheelVels.rfRot * 1000 * 180 / M_PI), + static_cast(wheelVels.lbRot * 1000 * 180 / M_PI), + static_cast(wheelVels.rbRot * 1000 * 180 / M_PI)}; } case DriveMode::Crab: return {90000, 90000, 90000, 90000}; diff --git a/src/network/MissionControlProtocol.cpp b/src/network/MissionControlProtocol.cpp index d74852c3..cec6d264 100644 --- a/src/network/MissionControlProtocol.cpp +++ b/src/network/MissionControlProtocol.cpp @@ -177,7 +177,7 @@ void MissionControlProtocol::handleTankDriveRequest(const json& j) { } void MissionControlProtocol::handleTurnInPlaceDriveRequest(const json& j) { - double steer = j["steer"].get(); + double steer = -j["steer"].get(); double dtheta = Constants::MAX_DTHETA * steer; LOG_F(1, "{steer=%.2f} -> setTurnInPlaceCmdVel(%.4f)", steer, dtheta); this->setRequestedTurnInPlaceCmdVel(dtheta); diff --git a/src/world_interface/real_world_interface.cpp b/src/world_interface/real_world_interface.cpp index bfc44fa2..407694a9 100644 --- a/src/world_interface/real_world_interface.cpp +++ b/src/world_interface/real_world_interface.cpp @@ -66,6 +66,9 @@ std::unordered_map callbackIDMap; void initMotors() { for (const auto& it : motorSerialIDMap) { can::motor::initMotor(it.second); + bool hasPosSensor = robot::potMotors.find(it.first) != robot::potMotors.end() || + robot::encMotors.find(it.first) != robot::encMotors.end(); + addMotorMapping(it.first, hasPosSensor); } for (const auto& pot_motor_pair : robot::potMotors) { @@ -76,9 +79,6 @@ void initMotors() { can::motor::initPotentiometer(serial, pot_params.mdeg_lo, pot_params.mdeg_hi, pot_params.adc_lo, pot_params.adc_hi, TELEM_PERIOD); - - // initialize motor objects and add them to map - addMotorMapping(motor_id, true); } for (const auto& enc_motor_pair : robot::encMotors) { @@ -91,17 +91,8 @@ void initMotors() { TELEM_PERIOD); can::motor::setLimitSwitchLimits(serial, enc_params.limitSwitchLow, enc_params.limitSwitchHigh); - - // initialize motor objects and add them to map - addMotorMapping(motor_id, true); } - // initialize motor objects and add them to map - addMotorMapping(motorid_t::frontLeftWheel, false); - addMotorMapping(motorid_t::frontRightWheel, false); - addMotorMapping(motorid_t::rearLeftWheel, false); - addMotorMapping(motorid_t::rearRightWheel, false); - for (const auto& pair : robot::motorPIDMap) { motorid_t motor = pair.first; can::deviceserial_t serial = motorSerialIDMap.at(motor); @@ -110,7 +101,6 @@ void initMotors() { } can::motor::initMotor(motorSerialIDMap.at(motorid_t::hand)); - addMotorMapping(motorid_t::hand, false); } void openCamera(CameraID camID, const char* cameraPath) {