diff --git a/build.gradle b/build.gradle index 1380276..0fac042 100644 --- a/build.gradle +++ b/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2023.3.2" + id "edu.wpi.first.GradleRIO" version "2023.4.2" } // Define my targets (RoboRIO) and artifacts (deployable files) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 4caf50b..83f1877 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -18,11 +18,20 @@ #include "external/cpptoml.h" +#include + +#include +#include +#include + void Robot::RobotInit() { try{ c_toml = cpptoml::parse_file(frc::filesystem::GetDeployDirectory()+"/config.toml"); } catch (cpptoml::parse_exception & ex){ - std::cerr << "Unable to open file: config.toml" << std::endl; + std::cerr << "Unable to open file: config.toml" + << std::endl + << ex.what() + << std::endl; exit(1); } @@ -31,11 +40,36 @@ void Robot::RobotInit() { c_operatorController = new frc::XboxController(Interfaces::k_operatorXboxController); //Subsystems - c_drivetrain = new Drivetrain(false); + c_drivetrain = new Drivetrain(true); c_odometry = new Odometry(c_drivetrain); + //c_arm = new ArmSubsystem(c_toml->get_table("arm")); //Commands + //c_armTeleopCommand = new ArmTeleopCommand(c_arm, c_operatorController); c_driveTeleopCommand = new DriveTeleopCommand(c_drivetrain, c_driverController); + + auto orientWheels = frc2::StartEndCommand{ + //dump cube and move auto + [&] () { c_drivetrain->setMotion(0,0.05,0); }, + [&] () { c_drivetrain->setMotion(0,0,0); }, + { c_drivetrain } + }.ToPtr(); + auto forceOffCube = frc2::StartEndCommand{ + [&] () { c_drivetrain->setMotion(0,0.5,0); }, + [&] () { c_drivetrain->setMotion(0,0,0); }, + { c_drivetrain } + }.ToPtr(); + + auto putCubeIntoStation = frc2::StartEndCommand{ + [&] () { c_drivetrain->setMotion(0,-0.15,0); }, + [&] () { c_drivetrain->setMotion(0,0,0); }, + {c_drivetrain} + }.ToPtr(); + + c_autoDumpCubeAndScore = std::move(orientWheels).WithTimeout(0.5_s) + .AndThen(std::move(forceOffCube).WithTimeout(0.3_s)) + .AndThen(std::move(putCubeIntoStation).WithTimeout(2.0_s)) + .Unwrap(); } /** @@ -48,6 +82,8 @@ void Robot::RobotInit() { */ void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); + + frc::SmartDashboard::PutBoolean("Field Centric Enabled",c_drivetrain->getFieldCentric()); } /** @@ -64,16 +100,23 @@ void Robot::DisabledPeriodic() {} * RobotContainer} class. */ void Robot::AutonomousInit() { + c_autoDumpCubeAndScore->Schedule(); // TODO: Make sure to cancel autonomous command in teleop init. } -void Robot::AutonomousPeriodic() {} +void Robot::AutonomousPeriodic() { + c_drivetrain->Periodic();// update drivetrain no matter what +} void Robot::TeleopInit() { - // TODO: Make sure autonomous command is canceled first. + // Make sure autonomous command is canceled first. + c_autoDumpCubeAndScore->Cancel(); + + //c_armTeleopCommand->Schedule(); + //c_armTeleopCommand->resetTarget(); c_driveTeleopCommand->Schedule(); + c_drivetrain->enableFieldCentric(); } - /** * This function is called periodically during operator control. */ @@ -87,6 +130,10 @@ void Robot::TeleopPeriodic() { if (c_driverController->GetBButtonPressed()) { c_drivetrain->resetNavxHeading(); } + + if(c_driverController->GetXButtonPressed()) { + c_drivetrain->lockMovement(false); + } } /** diff --git a/src/main/cpp/commands/arm/ArmTeleopCommand.cpp b/src/main/cpp/commands/arm/ArmTeleopCommand.cpp new file mode 100644 index 0000000..fd60e11 --- /dev/null +++ b/src/main/cpp/commands/arm/ArmTeleopCommand.cpp @@ -0,0 +1,135 @@ +#include "commands/arm/ArmTeleopCommand.h" +#include "subsystems/arm/arm.h" + +#include + +#include + +#define JOYSTICK_DEADZONE 0.1 +#define DEADZONE(input) ((std::abs(input) < JOYSTICK_DEADZONE) ? 0.0 : input) + +const double k_maxPointSpeed = 0.005; +const double k_maxPointRotSpeed = (2.0 * M_PI / 10.0) * 0.02; // radians per second in 20ms. +const double k_maxWristRotSpeed = (2.0 * M_PI / 2.0) * 0.02; +const double k_maxGripSpeed = 0.1; + +ArmTeleopCommand::ArmTeleopCommand(ArmSubsystem* arm, frc::XboxController* operatorController) { + c_operatorController = operatorController; + c_arm = arm; + + AddRequirements(c_arm); +} + +void ArmTeleopCommand::Initialize() { + // Update target to current point so arm doesn't move unexpectedly. + m_target = c_arm->getGripPoint(); + c_arm->moveToPoint(m_target); +} + +void ArmTeleopCommand::Execute() { + { + Vector offsetLX; + Vector offsetLY; + Vector offsetRY; + double gripMag = std::sqrt(std::pow(m_target.x,2)+std::pow(m_target.y,2)); + double gripDir = std::atan2(m_target.x, m_target.y); // 0 deg == y axis + + // Rotate turret. Speed of rotation is reduced the further the arm reaches. + double leftX = c_operatorController->GetLeftX(); + // Square input to improve fidelity. + leftX = DEADZONE(leftX); + leftX = std::copysign(std::clamp(leftX * leftX, -1.0, 1.0), leftX); + if (0.0 != leftX) { + // (+) leftX should move turret clockwise. + double dir = gripDir + (leftX * k_maxPointRotSpeed) / std::max(gripMag,1.0); + Point point1( + gripMag * std::sin(dir), + gripMag * std::cos(dir), + m_target.z + ); + offsetLX = point1 - m_target; + } + + // Extend/retract gripper from/to turret. + double leftY = -c_operatorController->GetLeftY(); /* Invert so + is forward */ + // Square input to improve fidelity. + leftY = DEADZONE(leftY); + leftY = std::copysign(std::clamp(leftY * leftY, -1.0, 1.0), leftY); + if (0.0 != leftY) { + // (+) leftY should move away from turret. + double mag = gripMag + (leftY * k_maxPointSpeed); + Point point1( + mag * std::sin(gripDir), + mag * std::cos(gripDir), + m_target.z + ); + offsetLY = point1 - m_target; + } + + // Move gripper up or down. + double rightY = -c_operatorController->GetRightY(); /* Invert so + is up */ + // Square input to improve fidelity. + rightY = DEADZONE(rightY); + rightY = std::copysign(std::clamp(rightY * rightY, -1.0, 1.0), rightY); + if (0.0 != rightY) { + // (+) rightY should move gripper up. + offsetRY = Vector(0.0, 0.0, rightY * k_maxPointSpeed); + } + + m_target = m_target + offsetLX + offsetLY + offsetRY; + + c_arm->moveToPoint(m_target); + } + + // Rotate wrist clockwise or counterclockwise. + { + double leftTrigger = c_operatorController->GetLeftTriggerAxis(); + double rightTrigger = c_operatorController->GetRightTriggerAxis(); + double trigger = leftTrigger - rightTrigger; + // Square input to improve fidelity. + trigger = DEADZONE(trigger); + trigger = std::copysign(std::clamp(trigger * trigger, -1.0, 1.0), trigger); + + // Use current angle as default so wrist doesn't move wildly upon enable. + double wristTargetAngle = c_arm->getWristRollAngle(); + + if (0.0 != trigger) { + // (+) trigger should rotate wrist counter-clockwise, looking down forearm. + wristTargetAngle += trigger * k_maxWristRotSpeed; + } + + // Move/hold wrist angle. + c_arm->setWristRollAngle(wristTargetAngle); + } + + // Open and close gripper. + { + double bumper = + (c_operatorController->GetRightBumper() ? 1.0 : 0.0) + - (c_operatorController->GetLeftBumper() ? 1.0 : 0.0); + + // Use current position as default so grip doesn't move wildly upon enable. + double gripTargetPos = c_arm->getGrip(); + + if (0.0 != bumper) { + gripTargetPos += bumper * k_maxGripSpeed; + } + + // Move/hold grip. + c_arm->setGrip(gripTargetPos); + } +} + +void ArmTeleopCommand::End(bool interrupted) { + // Update target to current point so arm stops. + // Cannot stop motors because shoulder will backdrive and fall. + m_target = c_arm->getGripPoint(); +} + +bool ArmTeleopCommand::IsFinished() { + return false; // dont end because then we wont be able to drive +} + +void ArmTeleopCommand::resetTarget(){ + m_target = c_arm->getGripPoint(); +} \ No newline at end of file diff --git a/src/main/cpp/commands/arm/MoveToIntake.cpp b/src/main/cpp/commands/arm/MoveToIntake.cpp new file mode 100644 index 0000000..e79a924 --- /dev/null +++ b/src/main/cpp/commands/arm/MoveToIntake.cpp @@ -0,0 +1,43 @@ +#include "commands/arm/MoveToIntake.h" + +#include "subsystems/arm/motionPath.h" +#include "subsystems/arm/arm.h" +#include "subsystems/arm/armPose.h" + +MoveToIntakeCommand::MoveToIntakeCommand(ArmSubsystem * arm, Point currentPoint) { + m_arm = arm; + m_currentPoint = currentPoint; + + AddRequirements(m_arm); +} + +void MoveToIntakeCommand::Initialize() { + m_finalTarget = m_arm->m_pointIntake; + Point currentPoint = m_arm->getGripPoint(); + + m_path.emplace(std::move(m_arm->getPathTo(currentPoint, m_finalTarget))); + m_target = m_path->getNextPoint(); +} + +void MoveToIntakeCommand::Execute() { + if(m_finalTarget.isNear(m_target) && m_arm->isNearPoint(m_target)) { + return; + } else if (m_arm->isNearPoint(m_target)) { + m_target = m_path->getNextPoint(); + } + + if (!m_arm->isPointSafe(m_target)) { + return; + } + + ArmPose calculatedIKPose = m_arm->calcIKJointPoses(m_target); +} + +void MoveToIntakeCommand::End(bool isInterrupted) { + //heeyaw +} + +bool MoveToIntakeCommand::IsFinished() { + return !m_arm->isPointSafe(m_target) + || (m_finalTarget.isNear(m_target) && m_arm->isNearPoint(m_target)); +} diff --git a/src/main/cpp/commands/drivetrain/driveTeleopCommand.cpp b/src/main/cpp/commands/drivetrain/driveTeleopCommand.cpp index e4ea2b6..da71c04 100644 --- a/src/main/cpp/commands/drivetrain/driveTeleopCommand.cpp +++ b/src/main/cpp/commands/drivetrain/driveTeleopCommand.cpp @@ -34,8 +34,9 @@ void DriveTeleopCommand::Execute() { // the rotation limit is there in case the driver does not want to spin as fast while driving (specifically limiting the controller input) c_drivetrain->setMotion( - -DEADZONE(c_driverController->GetLeftX()) * speedFactor, - DEADZONE(c_driverController->GetLeftY()) * speedFactor, + DEADZONE(c_driverController->GetLeftX()) * speedFactor, + -DEADZONE(c_driverController->GetLeftY()) * speedFactor, + // Reverse rotation so right is clockwise robot motion. DEADZONE(c_driverController->GetRightX()) * Constants::k_maxSpinSpeed ); } diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp new file mode 100644 index 0000000..9ac3d50 --- /dev/null +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -0,0 +1,432 @@ +#include "Mandatory.h" +#include "subsystems/arm/arm.h" +#include "util/math.h" + +#include +#include +#include +#include + + +// PROTOTYPES +static double requireTomlDouble (std::shared_ptr toml, std::string const & name); + +// CONSTANTS +const double k_ChassisXSize = 0.775; // meters +const double k_ChassisYSize = 0.826; // meters +const double k_ChassisZSize = 0.229; // meters + +const double k_TurretZOffset = 0.210; // meters + +const double k_PickupXSize = 0.076; // meters +const double k_PickupYSize = 0.340; // meters +const double k_PickupZSize = 1.000; // meters + +ArmSubsystem::ArmSubsystem(std::shared_ptr toml) { + loadConfig(toml); + + double kp, ki, kd, tolerance; + + kp = requireTomlDouble(toml, "shoulder.pid.p"); + ki = requireTomlDouble(toml, "shoulder.pid.i"); + kd = requireTomlDouble(toml, "shoulder.pid.d"); + tolerance = requireTomlDouble(toml, "shoulder.pid.tolerance"); + + m_shoulderPid = new frc2::PIDController(kp, ki, kd); + m_shoulderPid->SetTolerance(tolerance); + m_shoulderPid->SetIntegratorRange(-0.01, 0.01); + + m_shoulderPid->SetSetpoint(getShoulderAngle()); + + m_turretMotor.SetSmartCurrentLimit(5.0); + m_lowJointMotor.SetSmartCurrentLimit(5.0); + m_midJointMotor.SetSmartCurrentLimit(5.0); + m_gripperRotateMotor.SetSmartCurrentLimit(5.0); + m_gripperGraspMotor.SetSmartCurrentLimit(5.0); +} + +void ArmSubsystem::Periodic() { + // Move shoulder or hold position. + if (nullptr != m_shoulderPid) { + double output = m_shoulderPid->Calculate(getShoulderAngle()); + // Reverse motor direction. + output = -output; + output = std::clamp(output, -0.2, 0.2); + m_lowJointMotor.Set(output); + } + + + + // Report calculated gripper position. + + Point gripPos = getGripPoint(); + std::stringstream gripPosStr; + + + // Report calculated arm pose angles. What the robot thinks the angles + // should be to reach gripper position. +} + +void ArmSubsystem::initialiseBoundary() { + // Geometric Conventions: + // x-axis 0 :: center of arm turret. + // y-axis 0 :: center of arm turret. + // z-axis 0 :: floor. + // + // x+ :: robot right side (with pickup gap in bumpers). + // y+ :: robot front (furthest side from turret). + // z+ :: up. + + // Only create boundaries for no-go zones. + // Otherwise, the code gets really complicated later + // to check for collisions. + + std::shared_ptr chassisNoGoBounds = std::make_shared(std::vector>{ + std::make_shared( + -(k_ChassisXSize / 2.0), (k_ChassisXSize / 2.0), + (k_PickupYSize / 2.0) , (k_ChassisYSize / 2.0), + 0.0 , (k_ChassisZSize) + ), + std::make_shared( + -(k_ChassisXSize / 2.0), (k_ChassisXSize / 2.0) - k_PickupXSize, + -(k_PickupYSize / 2.0) , (k_PickupYSize / 2.0), + 0.0 , (k_ChassisZSize) + ), + std::make_shared( + -(k_ChassisXSize / 2.0), (k_ChassisXSize / 2.0), + -(k_ChassisYSize / 2.0), -(k_PickupYSize / 2.0), + 0.0 , (k_ChassisZSize) + ) + }); + + std::shared_ptr cannotReachNoGoBounds = std::make_shared( + std::make_unique( + Point { 0.0, 0.0, k_ChassisZSize }, /* center of turret */ + (Constants::Arm::k_forearmLenMeters + + Constants::Arm::k_bicepLenMeters + ) * 0.95 /* 95% of max length of arm */ + ) + ); + + std::shared_ptr turretNoGoZone = std::make_shared( + Point { 0.0, 0.0, 0.0 }, + 0.08, /* radius meters */ + 0.0, 1.0 + ); + + std::shared_ptr floorNoGoZone = std::make_shared( + -5.0, 5.0, + -5.0, 5.0, + -5.0, 0.0254 /* keep gripper 1 inch from floor */ + ); + + std::shared_ptr defNoGoZone = std::make_shared(std::vector{ + std::move(cannotReachNoGoBounds), + std::move(chassisNoGoBounds), + std::move(turretNoGoZone), + std::move(floorNoGoZone), + }); + + m_noGoZone = std::move(defNoGoZone); +} + +Point ArmSubsystem::calcElbowPos(double turretAng, double shoulderAng) { + // Origin z is floor level. Add turret z offset to calculated point. + Point pt = Point( + Constants::Arm::k_bicepLenMeters * std::cos(shoulderAng) * std::sin(turretAng), + Constants::Arm::k_bicepLenMeters * std::cos(shoulderAng) * std::cos(turretAng), + Constants::Arm::k_bicepLenMeters * std::sin(shoulderAng) + ) + Vector(0.0, 0.0, k_TurretZOffset); + + return pt; +} + +Point ArmSubsystem::calcGripPos( + double turretAng, + double shoulderAng, + double elbowAng +) { + Point elbowPos = calcElbowPos(turretAng, shoulderAng); + Point pt( + Constants::Arm::k_forearmLenMeters * std::cos(shoulderAng + elbowAng) * std::sin(turretAng), + Constants::Arm::k_forearmLenMeters * std::cos(shoulderAng + elbowAng) * std::cos(turretAng), + Constants::Arm::k_forearmLenMeters * std::sin(shoulderAng + elbowAng) + ); + + return Point(elbowPos.x - pt.x, elbowPos.y - pt.y, elbowPos.z - pt.z); +} + +ArmPose ArmSubsystem::calcIKJointPoses(Point const & pt) { + // See arm.h for diagram of robot arm angle reference conventions. + // desmos 2d IK solver demo: https://www.desmos.com/calculator/p3uouu2un2 + + // IK solver assumes base of turret is origin. Adjust target point from + // robot origin to turret origin. + Point turretPt = pt - Vector(0.0, 0.0, k_TurretZOffset); + + double targetLen = std::sqrt( + std::pow(turretPt.x, 2) + + std::pow(turretPt.y, 2) + + std::pow(turretPt.z, 2) + ); // Line from shoulder to target + + // Constrain target point. + double constrainLen = std::min( + Constants::Arm::k_forearmLenMeters + Constants::Arm::k_bicepLenMeters * 0.95, /* Safety margin. Limit length to 95% of max. */ + targetLen + ); + + Point cp = Point(0.0, 0.0, 0.0) + Vector(turretPt.x, turretPt.y, turretPt.z).unit() * constrainLen; + + double c3 = atan2(cp.z, std::sqrt(cp.x * cp.x + cp.y * cp.y)); + + double c1 = std::acos( + (Constants::Arm::k_bicepLenMeters * Constants::Arm::k_bicepLenMeters + - Constants::Arm::k_forearmLenMeters * Constants::Arm::k_forearmLenMeters + + constrainLen * constrainLen) + / (2.0 * Constants::Arm::k_bicepLenMeters * constrainLen) + ); + + // Solve IK: + double shoulderAngle = c1 + c3; + + double elbowAngle = + std::numbers::pi + - std::acos( + ( + Constants::Arm::k_forearmLenMeters * Constants::Arm::k_forearmLenMeters + - Constants::Arm::k_bicepLenMeters * Constants::Arm::k_bicepLenMeters + + constrainLen * constrainLen + ) + / (2.0 * Constants::Arm::k_forearmLenMeters * constrainLen) + ) + - c1; + + double turretAngle = std::atan2(cp.x, cp.y); + + return ArmPose(turretAngle, shoulderAngle, elbowAngle); + +} + +bool ArmSubsystem::isPointSafe(Point const & point) { + if (nullptr == m_noGoZone) { + return false; + } else { + return m_noGoZone->isOutside(point); + } +} + +bool ArmSubsystem::isNearPoint(Point const & point) { + Point currentPosition = getGripPoint(); + + return point.isNear(currentPosition); +} + +MotionPath ArmSubsystem::getPathTo(Point const & current, Point const & target) { + // FIXME: compute proper path sequence for arm to follow. + return MotionPath({ target }); +} + +// ============ Point Grabbers: ============ //| + +Point const & ArmSubsystem::getIntakePoint() { // | Intake + return ArmSubsystem::m_pointIntake; +} +Point const & ArmSubsystem::getHomePoint() { // | Home + return ArmSubsystem::m_pointHome; +} +Point const & ArmSubsystem::getHybridPoint() { // | Hybrid + return ArmSubsystem::m_pointHybrid; +} +Point const & ArmSubsystem::getLowPolePoint() { // | Low Pole + return ArmSubsystem::m_pointLowPole; +} +Point const & ArmSubsystem::getHighPolePoint() {// | High Pole + return ArmSubsystem::m_pointHighPole; +} +Point const & ArmSubsystem::getLowShelfPoint() {// | Low Shelf + return ArmSubsystem::m_pointLowShelf; +} +Point const & ArmSubsystem::getHighShelfPoint() {// | High Shelf + return ArmSubsystem::m_pointHighShelf; +} + +// Getting and setting arm angles: + +double ArmSubsystem::getTurretAngle() { + return m_turretAngleSensor.Get() * config.turret.sensorToRadians + config.turret.zeroOffset; +} + +double ArmSubsystem::getShoulderAngle() { + return m_shoulderAngleSensor.Get().value() * 2.0 * M_PI + config.shoulder.zeroOffset; +} + +double ArmSubsystem::getElbowAngle() { + return m_elbowAngleSensor.Get() * config.elbow.sensorToRadians + config.elbow.zeroOffset; +} + +double ArmSubsystem::getWristRollAngle() { + return m_wristRollAngleSensor.Get() * config.wrist.sensorToRadians + config.wrist.zeroOffset; +} + +double ArmSubsystem::getGrip() { + return m_gripSensor.Get() * config.grip.sensorToMeters + config.grip.zeroOffset; +} + +Point ArmSubsystem::getGripPoint() { + return calcGripPos( + getTurretAngle(), + getShoulderAngle(), + getElbowAngle() + ); +} + +Point const & ArmSubsystem::getSafetyPoint(Point pt) { + if (isNearZero(pt.x)) { + return ArmSubsystem::m_safetyPointCenter; + } else if (0.0 > pt.x) { + return ArmSubsystem::m_safetyPointGrid; + } else { + return ArmSubsystem::m_safetyPointIntake; + } +} + +void ArmSubsystem::setTurretAngle(double angle) { + angle = std::clamp(angle, config.turret.limit.lo, config.turret.limit.hi); + + double da = angle - getTurretAngle(); + if (isNearZero(da, 0.02)) { + m_turretMotor.Set(0.0); + } else { + m_turretMotor.Set(std::clamp(da, -0.1, 0.1)); + } +} + +void ArmSubsystem::setShoulderAngle(double angle) { + angle = std::clamp(angle, config.shoulder.limit.lo, config.shoulder.limit.hi); + + /* + double da = angle - getShoulderAngle(); + if (isNearZero(da)) { + m_lowJointMotor.Set(0.0); + } else { + m_lowJointMotor.Set(std::copysign(0.05, da)); + } + */ + + // Use PID to hold arm in place once target angle is reached. + m_shoulderPid->SetSetpoint(angle); +} + +void ArmSubsystem::setElbowAngle(double angle) { + angle = std::clamp(angle, config.elbow.limit.lo, config.elbow.limit.hi); + + double da = angle - getElbowAngle(); + if (isNearZero(da, 0.02)) { + m_midJointMotor.Set(0.0); + } else { + m_midJointMotor.Set(std::clamp(da, -0.1, 0.1)); + } +} + +void ArmSubsystem::setWristRollAngle(double angle) { + angle = std::clamp(angle, config.wrist.limit.lo, config.wrist.limit.hi); + + double da = angle - getWristRollAngle(); + if (isNearZero(da, 0.01)) { + m_gripperRotateMotor.Set(0.0); + } else { + m_gripperRotateMotor.Set(std::clamp( + da + std::copysign(0.05, da), + -0.2, 0.2 + )); + } +} + +void ArmSubsystem::setGrip(double grip) { + grip = std::clamp(grip, config.grip.limit.lo, config.grip.limit.hi); + + double dx = grip - getGrip(); + if (isNearZero(dx, 0.015 /* meters */)) { + m_gripperGraspMotor.Set(0.0); + } else { + m_gripperGraspMotor.Set(std::clamp( + dx + std::copysign(0.02, dx), + -0.2, 0.2 + )); + } +} + +bool ArmSubsystem::moveToPoint(Point const & target) { + if (!isPointSafe(target)) { + return false; + } + + ArmPose pose = calcIKJointPoses(target); + + setTurretAngle(pose.turretAngle); + setShoulderAngle(pose.shoulderAngle); + setElbowAngle(pose.elbowAngle); + + return true; +} + +// NOT IMPLEMENTED IN HARDWARE +// float ArmSubsystem::getWristPitchAngle() { +// return 0; // TODO when we get reading components available +// } + +/// LOCAL FREE +static double requireTomlDouble ( + std::shared_ptr toml, + std::string const & name +) { + auto val = toml->get_qualified_as(name); + + if (val) { + return *val; + } else { + std::stringstream ss; + ss << "Missing config option: arm." << name; + throw ss; + } +} + +/// PRIVATE +void ArmSubsystem::loadConfig(std::shared_ptr toml) { + // Load zero offsets. + + config.turret.zeroOffset = requireTomlDouble(toml, "turret.zeroOffsetRadians"); + config.shoulder.zeroOffset = requireTomlDouble(toml, "shoulder.zeroOffsetRadians"); + config.elbow.zeroOffset = requireTomlDouble(toml, "elbow.zeroOffsetRadians"); + config.wrist.zeroOffset = requireTomlDouble(toml, "wrist.zeroOffsetRadians"); + config.grip.zeroOffset = requireTomlDouble(toml, "grip.zeroOffsetMeters"); + + // Load conversion factors. + + config.turret.sensorToRadians = requireTomlDouble(toml, "turret.sensorToRadians"); + // No conversion factor for shoulder since sensor units are units::turns_t. + config.elbow.sensorToRadians = requireTomlDouble(toml, "elbow.sensorToRadians"); + config.wrist.sensorToRadians = requireTomlDouble(toml, "wrist.sensorToRadians"); + config.grip.sensorToMeters = requireTomlDouble(toml, "grip.sensorToMeters"); + + // Load raw limits and convert to radians or meters. + + config.turret.limit.lo = DEG_2_RAD(requireTomlDouble(toml, "turret.limit.lo")); + config.turret.limit.hi = DEG_2_RAD(requireTomlDouble(toml, "turret.limit.hi")); + + config.shoulder.limit.lo = DEG_2_RAD(requireTomlDouble(toml, "shoulder.limit.lo")); + config.shoulder.limit.hi = DEG_2_RAD(requireTomlDouble(toml, "shoulder.limit.hi")); + + config.elbow.limit.lo = DEG_2_RAD(requireTomlDouble(toml, "elbow.limit.lo")); + config.elbow.limit.hi = DEG_2_RAD(requireTomlDouble(toml, "elbow.limit.hi")); + + config.wrist.limit.lo = DEG_2_RAD(requireTomlDouble(toml, "wrist.limit.lo")); + config.wrist.limit.hi = DEG_2_RAD(requireTomlDouble(toml, "wrist.limit.hi")); + + config.grip.limit.lo = requireTomlDouble(toml, "grip.limit.lo"); + config.grip.limit.hi = requireTomlDouble(toml, "grip.limit.hi"); + + config.grip.setpoint.open = requireTomlDouble(toml, "grip.setpoint.open"); + config.grip.setpoint.close = requireTomlDouble(toml, "grip.setpoint.close"); +} diff --git a/src/main/cpp/subsystems/arm/motionPath.cpp b/src/main/cpp/subsystems/arm/motionPath.cpp new file mode 100644 index 0000000..5a8dff3 --- /dev/null +++ b/src/main/cpp/subsystems/arm/motionPath.cpp @@ -0,0 +1,14 @@ +#include "subsystems/arm/motionPath.h" + +Point MotionPath::getNextPoint() { + // FIXME: return the next step in the path. + return m_finalTarget; +} + +Point MotionPath::interpolate(Point const & p0, Point const & p1) { + return Point(p0); +} + +bool MotionPath::isDone() { + return m_path.size() == 1 && m_finalTarget.isNear(m_path[0]); +} \ No newline at end of file diff --git a/src/main/cpp/subsystems/drivetrain/drivetrain.cpp b/src/main/cpp/subsystems/drivetrain/drivetrain.cpp index 3f3e0c0..17fc3db 100644 --- a/src/main/cpp/subsystems/drivetrain/drivetrain.cpp +++ b/src/main/cpp/subsystems/drivetrain/drivetrain.cpp @@ -4,9 +4,6 @@ #define _USE_MATH_DEFINES #include -#include "util/point.h" -#include "util/polar.h" - #include "subsystems/drivetrain/swerveWheel.h" #include "subsystems/drivetrain/swerveWheelTypes.h" @@ -84,6 +81,9 @@ void Drivetrain::setupWheels() { } void Drivetrain::calculateWheelAnglesAndSpeeds() { + if(m_forceLockMovement){ + return; + } if ((abs(Drivetrain::m_strife) <= 0.001) && (abs(Drivetrain::m_forwards) <= 0.001)) { if(abs(Drivetrain::m_rotation) <= 0.001){ for (int i = 0; i < Constants::k_NumberOfSwerveModules; i++) { @@ -242,3 +242,16 @@ double Drivetrain::getMovementHeading(int module){ double Drivetrain::getMovementVelocity(int module){ return c_wheels[module]->getVelocity(); } + +void Drivetrain::lockMovement(bool restrictMovement){ + // if set true, skips the calculation of everything, and this stays in its orientation regardless of drive movement + m_forceLockMovement = restrictMovement; + m_strife = 0; // set everything to 0 as a safety + m_forwards = 0; + m_rotation = 0; + for(int i=0;i +#include "subsystems/arm/arm.h" #include "subsystems/drivetrain/drivetrain.h" #include "subsystems/drivetrain/odometry.h" + +#include "commands/arm/ArmTeleopCommand.h" #include "commands/drivetrain/driveTeleopCommand.h" #include "external/cpptoml.h" +//temporary auto +#include +#include + class Robot : public frc::TimedRobot { public: void RobotInit() override; @@ -47,7 +54,12 @@ class Robot : public frc::TimedRobot { //Subsystems Drivetrain* c_drivetrain = nullptr; Odometry* c_odometry = nullptr; + //ArmSubsystem* c_arm = nullptr; //Commands + //ArmTeleopCommand* c_armTeleopCommand = nullptr; DriveTeleopCommand* c_driveTeleopCommand = nullptr; + + //dump cube and put into scoring zone auto + std::unique_ptr c_autoDumpCubeAndScore{nullptr}; }; diff --git a/src/main/include/commands/arm/ArmTeleopCommand.h b/src/main/include/commands/arm/ArmTeleopCommand.h new file mode 100644 index 0000000..5353c66 --- /dev/null +++ b/src/main/include/commands/arm/ArmTeleopCommand.h @@ -0,0 +1,26 @@ +#pragma once + +#include "Mandatory.h" +#include "subsystems/arm/arm.h" +#include "util/geom.h" + +#include +#include + +#include + +class ArmTeleopCommand : public frc2::CommandHelper { +public: + ArmTeleopCommand(ArmSubsystem* arm, frc::XboxController* operatorController); + + void Initialize() override; + void Execute() override; + void End(bool interrupted) override; + bool IsFinished() override; + void resetTarget(); +private: + ArmSubsystem* c_arm; + frc::XboxController* c_operatorController; + + Point m_target; +}; \ No newline at end of file diff --git a/src/main/include/commands/arm/MoveToIntake.h b/src/main/include/commands/arm/MoveToIntake.h new file mode 100644 index 0000000..14bcde3 --- /dev/null +++ b/src/main/include/commands/arm/MoveToIntake.h @@ -0,0 +1,28 @@ +#pragma once + +#include "subsystems/arm/arm.h" +#include "subsystems/arm/motionPath.h" + +#include + +#include +#include + + +class MoveToIntakeCommand : public frc2::CommandHelper { +public: + MoveToIntakeCommand(ArmSubsystem * arm, Point finalTarget); + + void Initialize() override; + void Execute() override; + void End(bool interrupted) override; + bool IsFinished() override; + +private: + ArmSubsystem * m_arm = nullptr; + Point m_finalTarget; + Point m_target; + std::optional m_path; + + Point m_currentPoint; +}; diff --git a/src/main/include/subsystems/arm/arm.h b/src/main/include/subsystems/arm/arm.h new file mode 100644 index 0000000..09e6801 --- /dev/null +++ b/src/main/include/subsystems/arm/arm.h @@ -0,0 +1,204 @@ +#pragma once + +#include "Mandatory.h" +#include "subsystems/arm/armPose.h" +#include "subsystems/arm/boundary.h" +#include "subsystems/arm/motionPath.h" +#include "util/geom.h" + +#include "external/cpptoml.h" + +#include +#include + +#include +#include +#include +#include + +#include + +using namespace Interfaces::Arm; + +class ArmSubsystem : public frc2::SubsystemBase { +public: + ArmSubsystem(std::shared_ptr toml); + + void Periodic() override; + + void initialiseBoundary(); + + // Calulating Functions: + Point calcElbowPos(double turretAng, double angShoulder); + + Point calcGripPos(double turretAng, double shoulderAng, double elbowAng); + + ArmPose calcIKJointPoses(Point const & pt); + + bool isPointSafe(Point const & point); + bool isNearPoint(Point const & point); + + MotionPath getPathTo(Point const & current, Point const & target); + + // Reading Functions: + double getTurretAngle(); + double getShoulderAngle(); + double getElbowAngle(); + double getWristRollAngle(); + double getGrip(); + + // Getting Points: + Point getGripPoint(); + + Point const & getSafetyPoint(Point pt); + + Point const & getIntakePoint(); + Point const & getHomePoint(); + Point const & getHybridPoint(); + Point const & getLowPolePoint(); + Point const & getHighPolePoint(); + Point const & getLowShelfPoint(); + Point const & getHighShelfPoint(); + + void setTurretAngle(double angle); + void setShoulderAngle(double angle); + void setElbowAngle(double angle); + void setWristRollAngle(double angle); + void setGrip(double grip); // hehe grippy grabby hand + + /** + * Given a target position, compute necessary joint angles, and move joints + * of arm toward target. Will NOT stop on its own. Must call this method + * repeatedly to stop on target. + * + * @return false if point is within the no-go zone, true if point is safe. + */ + bool moveToPoint(Point const & target); + +private: + void loadConfig(std::shared_ptr toml); + + // Arm Diagram: + // 1: Turret, 2: Shoulder, 3: Elbow, 4: Wrist + // + // (3)---(4){ + // / + // [2] + // (1) + // [=====1720=====] + // O O + // **Not to scale + // Shoulder = Low Joint, Elbow = Mid Joint, Wrist = Gripper Rotate/Roll + // + // Turret angle conventions/reference + // 0 deg + // ┌──────────┐ + // │ │ + // │ │ + // │ │ + // -90 deg │ o │ 90 deg + // │ │ + // └──────────┘ + + rev::CANSparkMax m_turretMotor { + k_TurretMotor, + rev::CANSparkMaxLowLevel::MotorType::kBrushless + }; + rev::CANSparkMax m_lowJointMotor { + k_LowJointMotor, + rev::CANSparkMaxLowLevel::MotorType::kBrushless + }; + rev::CANSparkMax m_midJointMotor { + k_MidJointMotor, + rev::CANSparkMaxLowLevel::MotorType::kBrushless + }; + rev::CANSparkMax m_gripperRotateMotor { + k_GripperRotateMotor, + rev::CANSparkMaxLowLevel::MotorType::kBrushless + }; + rev::CANSparkMax m_gripperGraspMotor { + k_GripperGraspMotor, + rev::CANSparkMaxLowLevel::MotorType::kBrushless + }; + + frc::AnalogPotentiometer m_turretAngleSensor {k_TurretSensor, 1.0, 0.0}; + frc::AnalogPotentiometer m_elbowAngleSensor {k_ElbowSensor, 1.0, 0.0}; + frc::AnalogPotentiometer m_wristRollAngleSensor {k_WristRollSensor, 1.0, 0.0}; + frc::AnalogPotentiometer m_gripSensor {k_GripSensor, 1.0, 0.0}; + frc::DutyCycleEncoder m_shoulderAngleSensor{k_ShoulderSensor}; // Using Funky Fresh Encoder + + frc2::PIDController * m_shoulderPid = nullptr; + + std::shared_ptr m_noGoZone = nullptr; + + // Safety Points + Point m_safetyPointGrid{-0.2540, 0.0, 0.9144}; + Point m_safetyPointCenter{0.0, 0.254, 0.8635}; + Point m_safetyPointIntake{0.254, 0.0, -0.8128}; + + struct { + struct { + struct { + double lo; + double hi; + } limit; + + double zeroOffset; + double sensorToRadians; + } turret; + + struct { + struct { + double lo; + double hi; + } limit; + + double zeroOffset; + } shoulder; + + struct { + struct { + double lo; + double hi; + } limit; + + double zeroOffset; + double sensorToRadians; + } elbow; + + struct { + struct { + double lo; + double hi; + } limit; + + double zeroOffset; + double sensorToRadians; + } wrist; + + struct { + struct { + double lo; + double hi; + } limit; + + double zeroOffset; + double sensorToMeters; + + struct { + double open; + double close; + } setpoint; + } grip; + } config; + +public: + // Other Points + Point m_pointIntake{0.4064, 0, 0.1524}; + Point m_pointHome{0.0, 0.0, 0.0}; // ! ! Unknown ! ! + Point m_pointHybrid{-0.5715, 0.0, 0.2286}; + Point m_pointLowPole{-0.9589, 0.0, 1.0668}; + Point m_pointHighPole{-1.3907, 0.0, 1.2700}; + Point m_pointLowShelf{-0.9398, 0.0, 0.7620}; + Point m_pointHighShelf{-1.3335, 0.0, 1.0668}; +}; diff --git a/src/main/include/subsystems/arm/armPose.h b/src/main/include/subsystems/arm/armPose.h new file mode 100644 index 0000000..a0d74e0 --- /dev/null +++ b/src/main/include/subsystems/arm/armPose.h @@ -0,0 +1,13 @@ +#pragma once + +class ArmPose { + public: + ArmPose(float turretAngle, float shoulderAngle, float elbowAngle) + : turretAngle(turretAngle), + shoulderAngle(shoulderAngle), + elbowAngle(elbowAngle) {} + + float turretAngle; + float shoulderAngle; + float elbowAngle; +}; diff --git a/src/main/include/subsystems/arm/boundary.h b/src/main/include/subsystems/arm/boundary.h new file mode 100644 index 0000000..0cf5b8b --- /dev/null +++ b/src/main/include/subsystems/arm/boundary.h @@ -0,0 +1,181 @@ +#pragma once + +#include "util/geom.h" + +#include +#include + +class Boundary { + public: + virtual bool isInside(Point const & pt) { return true; } + virtual bool isOutside(Point const & pt) { return true; } + +}; + +class BoxBoundary : public Boundary { +public: + BoxBoundary( + double xLo, + double xHi, + double yLo, + double yHi, + double zLo, + double zHi) + : xLo(xLo), xHi(xHi), yLo(yLo), yHi(yHi), zLo(zLo), zHi(zHi) { } + + bool isInside(Point const & pt) override { + return (xLo < pt.x && pt.x < xHi) + && (yLo < pt.y && pt.y < yHi) + && (zLo < pt.z && pt.z < zHi); + } + + bool isOutside(Point const & pt) override { + return !isInside(pt); + } + +private: + double xLo; + double xHi; + double yLo; + double yHi; + double zLo; + double zHi; +}; + +// Polygon points must wind counter-clockwise. +class ConvexPolygonBoundary : public Boundary { +public: + ConvexPolygonBoundary(std::vector points, double zLo, double zHi) + : points(points), zLo(zLo), zHi(zHi) { + // Visit all points except the last point. + // Compute vector to following point. + for (unsigned int i = 0; i < points.size() - 1; i++) { + Vector v = points[i + 1] - points[i]; + vectors.push_back(Vector{-v.y, v.x, v.z}); + } + + // Visit the last point. + // Compute vector back to first point. + vectors.push_back(points[0] - points[points.size() - 1]); + } + + bool isInside(Point const & pt) override { + std::vector targets; + + for (unsigned int i = 0; i < points.size(); i++) { + targets.push_back(pt - points[i]); + } + + for (unsigned int i = 0; i < vectors.size(); i++) { + if (vectors[i].dot(targets[i]) < 0.0) { + return false; + } + } + + return true; + } + + bool isOutside(Point const & pt) override { + return !isInside(pt); + } + +private: + std::vector points; + std::vector vectors; + double zLo; + double zHi; +}; + +class CylinderBoundary : public Boundary { +public: + CylinderBoundary(Point center, double radius, double zLo, double zHi) + : center(center), radius(radius), zLo(zLo), zHi(zHi) {} + + bool isInside(Point const & pt) override { + // Check XY plane + Point offset((center.x - pt.x), (center.y - pt.y), 0.0); + double dist = + (offset.x * offset.x) + + (offset.y * offset.y); + + if (dist < radius * radius) { + return zLo < pt.z && pt.z < zHi; + } else { + return true; + } + } + + bool isOutside(Point const & pt) override { + return !isInside(pt); + } + +private: + Point center; + double radius; + double zLo; + double zHi; +}; + +class SphereBoundary : public Boundary { +public: + SphereBoundary(Point center, double radius) : center(center), radius(radius) { } + + bool isInside(Point const & pt) override { + Point offset((center.x - pt.x), (center.y - pt.y), (center.z - pt.z)); + + double dist = + (offset.x * offset.x) + + (offset.y * offset.y) + + (offset.z * offset.z); + + return (dist < radius * radius); + } + + bool isOutside(Point const & pt) override { + return !isInside(pt); + } + +private: + Point center; + double radius; +}; + +class ComposeBoundary : public Boundary { +public: + ComposeBoundary(std::vector> && boundaries) + : boundaries(boundaries) { } + + bool isInside(Point const & pt) override { + for (auto& bound : boundaries) { + if (bound->isInside(pt)) { + return true; + } + } + + return false; + } + + bool isOutside(Point const & pt) override { + return !isInside(pt); + } + +private: + std::vector> boundaries; +}; + +class NotBoundary : public Boundary { +public: + + NotBoundary(std::unique_ptr boundary) : m_Bound(std::move(boundary)) {} + + bool isInside(Point const & pt) override { + return m_Bound->isOutside(pt); + } + + bool isOutside(Point const & pt) override { + return !isInside(pt); + } + +private: + std::unique_ptr m_Bound; +}; diff --git a/src/main/include/subsystems/arm/motionPath.h b/src/main/include/subsystems/arm/motionPath.h new file mode 100644 index 0000000..c93ad28 --- /dev/null +++ b/src/main/include/subsystems/arm/motionPath.h @@ -0,0 +1,60 @@ +#pragma once + +#include "util/geom.h" + +#include + +class MotionPath { +public: + MotionPath(std::vector path) : m_path(path) { + m_finalTarget = m_path[m_path.size() - 1]; + } + + Point interpolate(Point const & pt0, Point const & pt1); + + Point getNextPoint(); + + bool isDone(); + + // ArmSubsystem::isNearPoint(m_target) && m_path.size() == 1; + +private: + std::vector m_path; + + Point m_finalTarget; + Point m_target; +}; + +// command: +// MotionPath path; +// Point finalTarget; +// Point target = path.getNextPoint(); +// ArmSubsystem arm; +// loop { +// if (finalTarget.isNearPoint(target) && arm.isNearPoint(target)) { +// break; +// } else if (arm.isNearPoint(target)) { +// target = path.getNextPoint(); +// } + +// use ik to get pose +// apply to subsystem + +// } + + +// command: +// MotionPath path; +// Point finalTarget; +// ArmSubsystem arm; + +// loop { +// if (path.isDone()) { +// break; +// } else { +// point = path.getNextPoint(); + +// usee ik to get pose +// apply to subsystem. +// } +// } diff --git a/src/main/include/subsystems/drivetrain/drivetrain.h b/src/main/include/subsystems/drivetrain/drivetrain.h index 8ab5bb6..d4c6d0b 100644 --- a/src/main/include/subsystems/drivetrain/drivetrain.h +++ b/src/main/include/subsystems/drivetrain/drivetrain.h @@ -4,7 +4,7 @@ #include #include "subsystems/drivetrain/swerveWheel.h" -#include "util/point.h" +#include "util/geom.h" #include "util/polar.h" #include @@ -121,6 +121,10 @@ class Drivetrain : public frc2::SubsystemBase { */ void resetNavxHeading(); + /** + * @param restrictMovement whether the Drivetrain should lock out any movement + */ + void lockMovement(bool restrictMovement); private: void setupWheels(); @@ -135,6 +139,8 @@ class Drivetrain : public frc2::SubsystemBase { double m_forwards = 0; double m_rotation = 0; + bool m_forceLockMovement = false; + SwerveWheel * c_wheels[Constants::k_NumberOfSwerveModules] = {nullptr}; Point c_wheelPositions[Constants::k_NumberOfSwerveModules] = { diff --git a/src/main/include/subsystems/drivetrain/odometry.h b/src/main/include/subsystems/drivetrain/odometry.h index 73db818..ac67d9c 100644 --- a/src/main/include/subsystems/drivetrain/odometry.h +++ b/src/main/include/subsystems/drivetrain/odometry.h @@ -3,7 +3,7 @@ #include "Mandatory.h" #include -#include "util/point.h" +#include "util/geom.h" #include "subsystems/drivetrain/drivetrain.h" class Odometry : frc2::SubsystemBase { diff --git a/src/main/include/util/geom.h b/src/main/include/util/geom.h new file mode 100644 index 0000000..569ca00 --- /dev/null +++ b/src/main/include/util/geom.h @@ -0,0 +1,12 @@ +#pragma once + +#include "util/point.h" +#include "util/vector.h" + +Vector operator- (Point const & lhs, Point const & rhs); + +Point operator+ (Point const & lhs, Vector const & rhs); +Point operator- (Point const & lhs, Vector const & rhs); + +Vector operator* (Vector const &, double); +Vector operator* (double, Vector const &); diff --git a/src/main/include/util/math.h b/src/main/include/util/math.h new file mode 100644 index 0000000..533bfc9 --- /dev/null +++ b/src/main/include/util/math.h @@ -0,0 +1,7 @@ +#pragma once + +// MACROS +#define RAD_2_DEG(rad) ((rad) * 180.0 / M_PI) +#define DEG_2_RAD(deg) ((deg) * M_PI / 180.0) + +bool isNearZero(double val, double tolerance = 0.001); diff --git a/src/main/include/util/point.h b/src/main/include/util/point.h index e1f2fc8..bbe321a 100644 --- a/src/main/include/util/point.h +++ b/src/main/include/util/point.h @@ -1,15 +1,27 @@ #pragma once +#include "util/vector.h" + +#include + +static const double NEAR_ZERO_METERS = 0.005; + class Point { public: - Point(float x, float y, float z) : x(x), y(y), z(z) {} + Point(double x, double y, double z) : x(x), y(y), z(z) {} Point() { // default constructor x = 0; y = 0; z = 0; } // default constructor - float x; - float y; - float z; + double x; + double y; + double z; + + bool isNear(Point const & rhs) const { + return std::abs(x - rhs.x) < NEAR_ZERO_METERS + && std::abs(y - rhs.y) < NEAR_ZERO_METERS + && std::abs(z - rhs.z) < NEAR_ZERO_METERS; + } }; diff --git a/src/main/include/util/vector.h b/src/main/include/util/vector.h new file mode 100644 index 0000000..cab48b1 --- /dev/null +++ b/src/main/include/util/vector.h @@ -0,0 +1,37 @@ +#pragma once + +#include "util/math.h" + +#include + +class Vector { +public: + Vector(double x, double y, double z) : x(x), y(y), z(z) {} + Vector() { // Heehoo Default Constructor + x = 0; + y = 0; + z = 0; + } + + double dot(Vector & rhs) { + return (x * rhs.x) + (y * rhs.y) + (z * rhs.z); + } + + double len() { + return std::sqrt(x * x + y * y + z * z); + } + + Vector unit() { + double m = len(); + + if (isNearZero(m)) { + return Vector(1.0, 0.0, 0.0); + } else { + return Vector(x / m, y / m, z / m); + } + } + + double x; + double y; + double z; +}; diff --git a/src/test/cpp/boundaryTest.cpp b/src/test/cpp/boundaryTest.cpp new file mode 100644 index 0000000..a658d9b --- /dev/null +++ b/src/test/cpp/boundaryTest.cpp @@ -0,0 +1,86 @@ +#include + +#include "util/point.h" +#include "subsystems/arm/boundary.h" + +TEST(BoxBoundaryTest, GivenPointInsideBox_IsInsideReturnsTrue) { + BoxBoundary boundary { + -1.0, 1.0, + -1.0, 1.0, + -1.0, 1.0 + }; + + Point insidePoint {0.0, 0.0, 0.0}; + + ASSERT_TRUE(boundary.isInside(insidePoint)); + ASSERT_FALSE(boundary.isOutside(insidePoint)); +} + +TEST(BoxBoundaryTest, GivenPointOutsideBox_IsInsideReturnsFalse) { + BoxBoundary boundary { + -1.0, 1.0, + -1.0, 1.0, + -1.0, 1.0 + }; + + Point outsidePoint {2.0, 0.0, 0.0}; + + ASSERT_FALSE(boundary.isInside(outsidePoint)); + ASSERT_TRUE(boundary.isOutside(outsidePoint)); +} + +TEST(ConvexPolygonBoundaryTest, GivenPointInsideTriangle_IsInsideReturnsTrue) { + ConvexPolygonBoundary boundary { + { + Point{0.0, 1.0, 0.0}, + Point{-1.0, -1.0, 0.0}, + Point{1.0, -1.0, 0.0} + }, + 0.0, 1.0 + }; + + Point inTrianglePoint {0.0, 0.0, 0.5}; + + ASSERT_TRUE(boundary.isInside(inTrianglePoint)); + ASSERT_FALSE(boundary.isOutside(inTrianglePoint)); +} + +TEST(ConvexPolygonBoundaryTest, GivenPointOutsideTriangle_IsInsideReturnsFalse) { + ConvexPolygonBoundary boundary { + { + Point{0.0, 1.0, 0.0}, + Point{-1.0, -1.0, 0.0}, + Point{1.0, -1.0, 0.0} + }, + 0.0, 1.0 + }; + + Point inTrianglePoint {0.0, 2.0, 0.5}; + + ASSERT_FALSE(boundary.isInside(inTrianglePoint)); + ASSERT_TRUE(boundary.isOutside(inTrianglePoint)); +} + +TEST(SphereBoundaryTest, GivenPointInsideSphere_IsInsideReturnsTrue) { + SphereBoundary boundary { + Point{0.0, 0.0, 0.0}, + 1.0 + }; + + Point insidePoint {0.0, 0.0, 0.5}; + + ASSERT_TRUE(boundary.isInside(insidePoint)); + ASSERT_FALSE(boundary.isOutside(insidePoint)); +} + +TEST(SphereBoundaryTest, GivenPointOutsideSphere_IsInsideReturnsFalse) { + SphereBoundary boundary { + Point{0.0, 0.0, 0.0}, + 1.0 + }; + + Point outsidePoint {2.0, 0.0, 0.0}; + + ASSERT_FALSE(boundary.isInside(outsidePoint)); + ASSERT_TRUE(boundary.isOutside(outsidePoint)); +}