diff --git a/src/TunePID.cpp b/src/TunePID.cpp index 9f6675d3..d0f4c46b 100644 --- a/src/TunePID.cpp +++ b/src/TunePID.cpp @@ -18,7 +18,8 @@ extern "C" { } enum class targetmode_t { - step, sinusoidal + step, + sinusoidal }; using namespace robot::types; @@ -145,7 +146,8 @@ int main(int argc, char** argv) { time_point tp = steady_clock::now(); time_point startTime = tp; - while (steady_clock::now() - startTime < 3 * milliseconds((int)(period * 1000))) { + while (steady_clock::now() - startTime < + 3 * milliseconds(static_cast(period * 1000))) { int32_t current_angle = can::motor::getMotorPosition(serial).getData(); double difference = (current_angle - angle_target) / 1000.0; acc_error += difference * difference; @@ -159,7 +161,7 @@ int main(int argc, char** argv) { if (mode == targetmode_t::step) { prescaled_target = round(prescaled_target); } - angle_target = (int32_t) round(amplitude * prescaled_target) + starting_angle; + angle_target = (int32_t)round(amplitude * prescaled_target) + starting_angle; can::motor::setMotorPIDTarget(serial, angle_target); diff --git a/src/autonomous/AutonomousTask.cpp b/src/autonomous/AutonomousTask.cpp index e3067abe..82fb8839 100644 --- a/src/autonomous/AutonomousTask.cpp +++ b/src/autonomous/AutonomousTask.cpp @@ -36,7 +36,7 @@ void AutonomousTask::navigate() { SLOW_DRIVE_THRESHOLD, DONE_THRESHOLD, CLOSE_TO_TARGET_DUR_VAL); - DiffDriveKinematics diffDriveKinematics(Constants::EFF_WHEEL_BASE); + kinematics::DiffDriveKinematics diffDriveKinematics(Constants::EFF_WHEEL_BASE); auto sleepUntil = std::chrono::steady_clock().now(); while (!cmd.isDone()) { @@ -50,8 +50,8 @@ void AutonomousTask::navigate() { cmd.setState(latestPos, robot::types::dataclock::now()); commands::command_t output = cmd.getOutput(); auto scaledVels = diffDriveKinematics.ensureWithinWheelSpeedLimit( - DiffDriveKinematics::PreferredVelPreservation::PreferThetaVel, output.xVel, - output.thetaVel, Constants::MAX_WHEEL_VEL); + kinematics::DiffDriveKinematics::PreferredVelPreservation::PreferThetaVel, + output.xVel, output.thetaVel, Constants::MAX_WHEEL_VEL); navtypes::point_t relTarget = util::toTransform(latestPos) * _waypoint_coords; LOG_F(INFO, "Relative Target: (%lf, %lf)", relTarget(0), relTarget(1)); LOG_F(INFO, "thetaVel: %lf", scaledVels(2)); diff --git a/src/camera/calibration.cpp b/src/camera/calibration.cpp index 4e12c949..c36f3a37 100644 --- a/src/camera/calibration.cpp +++ b/src/camera/calibration.cpp @@ -121,8 +121,16 @@ static void help(char** argv) { printf("\n%s", liveCaptureHelp); } -enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 }; -enum Pattern { CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID }; +enum { + DETECTION = 0, + CAPTURING = 1, + CALIBRATED = 2 +}; +enum Pattern { + CHESSBOARD, + CIRCLES_GRID, + ASYMMETRIC_CIRCLES_GRID +}; static double computeReprojectionErrors(const std::vector>& objectPoints, @@ -135,12 +143,12 @@ computeReprojectionErrors(const std::vector>& objectPoi double totalErr = 0, err; perViewErrors.resize(objectPoints.size()); - for (i = 0; i < (int)objectPoints.size(); i++) { + for (i = 0; i < static_cast(objectPoints.size()); i++) { projectPoints(cv::Mat(objectPoints[i]), rvecs[i], tvecs[i], cameraMatrix, distCoeffs, imagePoints2); err = norm(cv::Mat(imagePoints[i]), cv::Mat(imagePoints2), cv::NORM_L2); - int n = (int)objectPoints[i].size(); - perViewErrors[i] = (float)std::sqrt(err * err / n); + int n = static_cast(objectPoints[i].size()); + perViewErrors[i] = static_cast(std::sqrt(err * err / n)); totalErr += err * err; totalPoints += n; } @@ -264,7 +272,7 @@ saveCameraParams(const std::string& filename, cv::Size imageSize, cv::Size board fs << "per_view_reprojection_errors" << cv::Mat(reprojErrs); if (!rvecs.empty() || !reprojErrs.empty()) - fs << "nframes" << (int)std::max(rvecs.size(), reprojErrs.size()); + fs << "nframes" << static_cast(std::max(rvecs.size(), reprojErrs.size())); if (flags & cv::CALIB_FIX_ASPECT_RATIO) fs << "aspectRatio" << aspectRatio; @@ -293,8 +301,8 @@ saveCameraParams(const std::string& filename, cv::Size imageSize, cv::Size board if (!rvecs.empty() && !tvecs.empty()) { CV_Assert(rvecs[0].type() == tvecs[0].type()); - cv::Mat bigmat((int)rvecs.size(), 6, rvecs[0].type()); - for (int i = 0; i < (int)rvecs.size(); i++) { + cv::Mat bigmat(static_cast(rvecs.size()), 6, rvecs[0].type()); + for (int i = 0; i < static_cast(rvecs.size()); i++) { cv::Mat r = bigmat(cv::Range(i, i + 1), cv::Range(0, 3)); cv::Mat t = bigmat(cv::Range(i, i + 1), cv::Range(3, 6)); @@ -310,8 +318,9 @@ saveCameraParams(const std::string& filename, cv::Size imageSize, cv::Size board } if (!imagePoints.empty()) { - cv::Mat imagePtMat((int)imagePoints.size(), (int)imagePoints[0].size(), CV_32FC2); - for (int i = 0; i < (int)imagePoints.size(); i++) { + cv::Mat imagePtMat(static_cast(imagePoints.size()), + static_cast(imagePoints[0].size()), CV_32FC2); + for (int i = 0; i < static_cast(imagePoints.size()); i++) { cv::Mat r = imagePtMat.row(i).reshape(2, imagePtMat.cols); cv::Mat imgpti(imagePoints[i]); imgpti.copyTo(r); @@ -489,7 +498,7 @@ int main(int argc, char** argv) { return fprintf(stderr, "Could not initialize video (%d) capture\n", cameraId), -2; if (!imageList.empty()) - nframes = (int)imageList.size(); + nframes = static_cast(imageList.size()); if (capture.isOpened()) printf("%s", liveCaptureHelp); @@ -509,7 +518,7 @@ int main(int argc, char** argv) { cv::Mat view0; capture >> view0; view0.copyTo(view); - } else if (i < (int)imageList.size()) + } else if (i < static_cast(imageList.size())) view = cv::imread(imageList[i], 1); if (view.empty()) { @@ -563,9 +572,9 @@ int main(int argc, char** argv) { if (found) drawChessboardCorners(view, boardSize, cv::Mat(pointbuf), found); - std::string msg = mode == CAPTURING - ? "100/100" - : mode == CALIBRATED ? "Calibrated" : "Press 'g' to start"; + std::string msg = mode == CAPTURING ? "100/100" + : mode == CALIBRATED ? "Calibrated" + : "Press 'g' to start"; int baseLine = 0; cv::Size textSize = cv::getTextSize(msg, 1, 1, 1, &baseLine); cv::Point textOrigin(view.cols - 2 * textSize.width - 10, @@ -573,9 +582,10 @@ int main(int argc, char** argv) { if (mode == CAPTURING) { if (undistortImage) - msg = cv::format("%d/%d Undist", (int)imagePoints.size(), nframes); + msg = + cv::format("%d/%d Undist", static_cast(imagePoints.size()), nframes); else - msg = cv::format("%d/%d", (int)imagePoints.size(), nframes); + msg = cv::format("%d/%d", static_cast(imagePoints.size()), nframes); } putText(view, msg, textOrigin, 1, 1, @@ -590,7 +600,7 @@ int main(int argc, char** argv) { } imshow("Image View", view); - char key = (char)cv::waitKey(capture.isOpened() ? 50 : 500); + char key = static_cast(cv::waitKey(capture.isOpened() ? 50 : 500)); if (key == 27) break; @@ -622,14 +632,14 @@ int main(int argc, char** argv) { getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0), imageSize, CV_16SC2, map1, map2); - for (i = 0; i < (int)imageList.size(); i++) { + for (i = 0; i < static_cast(imageList.size()); i++) { view = cv::imread(imageList[i], 1); if (view.empty()) continue; // undistort( view, rview, cameraMatrix, distCoeffs, cameraMatrix ); remap(view, rview, map1, map2, cv::INTER_LINEAR); imshow("Image View", rview); - char c = (char)cv::waitKey(); + char c = static_cast(cv::waitKey()); if (c == 27 || c == 'q' || c == 'Q') break; } diff --git a/src/control_interface.cpp b/src/control_interface.cpp index 79c91242..15b139cd 100644 --- a/src/control_interface.cpp +++ b/src/control_interface.cpp @@ -24,7 +24,7 @@ using namespace std::chrono_literals; namespace robot { namespace { -jointpos_t commandedWristPos{0, 0}; +kinematics::jointpos_t commandedWristPos{0, 0}; std::mutex wristPosMutex; void setJointMotorPower(robot::types::jointid_t joint, double power); @@ -57,7 +57,7 @@ double setCmdVel(double dtheta, double dx) { } } - wheelvel_t wheelVels = driveKinematics().robotVelToWheelVel(dx, dtheta); + kinematics::wheelvel_t wheelVels = driveKinematics().robotVelToWheelVel(dx, dtheta); double lPWM = wheelVels.lVel / Constants::MAX_WHEEL_VEL; double rPWM = wheelVels.rVel / Constants::MAX_WHEEL_VEL; double maxAbsPWM = std::max(std::abs(lPWM), std::abs(rPWM)); @@ -92,7 +92,7 @@ double setTankCmdVel(double left, double right) { if (!Globals::swerveController.checkWheelRotation(DriveMode::Normal, curr_wheel_rots)) return 0; - wheelvel_t wheelVels = {left, right}; + kinematics::wheelvel_t wheelVels = {left, right}; double lPWM = wheelVels.lVel / Constants::MAX_WHEEL_VEL; double rPWM = wheelVels.rVel / Constants::MAX_WHEEL_VEL; double maxAbsPWM = std::max(std::abs(lPWM), std::abs(rPWM)); diff --git a/src/kinematics/SwerveDriveKinematics.h b/src/kinematics/SwerveDriveKinematics.h index 05173719..2c282621 100644 --- a/src/kinematics/SwerveDriveKinematics.h +++ b/src/kinematics/SwerveDriveKinematics.h @@ -3,7 +3,6 @@ #include "../navtypes.h" namespace kinematics { -using namespace navtypes; /** * Represents robot wheel velocities in robot reference frame, @@ -61,7 +60,7 @@ class SwerveDriveKinematics { * robot's reference frame. * @return the robot's translational and angular velocity in the robot's reference frame. */ - pose_t wheelVelToRobotVel(swervewheelvel_t wheelVel) const; + navtypes::pose_t wheelVelToRobotVel(swervewheelvel_t wheelVel) const; /** * Calculate the pose update in the local reference frame given the velocities of the diff --git a/src/world_interface/simulator_interface.cpp b/src/world_interface/simulator_interface.cpp index d600e5d0..d78ccbd0 100644 --- a/src/world_interface/simulator_interface.cpp +++ b/src/world_interface/simulator_interface.cpp @@ -233,10 +233,10 @@ void initSimServer(net::websocket::SingleClientWSServer& ws) { namespace robot { namespace { -DiffDriveKinematics drive_kinematics(Constants::EFF_WHEEL_BASE); +kinematics::DiffDriveKinematics drive_kinematics(Constants::EFF_WHEEL_BASE); } // namespace -const DiffDriveKinematics& driveKinematics() { +const kinematics::DiffDriveKinematics& driveKinematics() { return drive_kinematics; } diff --git a/src/world_interface/world_interface.h b/src/world_interface/world_interface.h index de108a29..c710c264 100644 --- a/src/world_interface/world_interface.h +++ b/src/world_interface/world_interface.h @@ -11,8 +11,6 @@ #include #include -using namespace kinematics; - /** * @namespace robot * @brief Collection of functions that allows interfacing with the hardware and the world.