Merge pull request #1 from Superbro525Alt/setpoints

worked on setpoints needs test

src/main/cpp/Gripper.cpp

@@ -10,6 +10,10 @@ void Gripper::OnUpdate(units::second_t dt) {
       voltage = 1_V;
       break;
 
+    case GripperState::kStop:
+      voltage = 0_V;
+      break;
+
     case GripperState::kIntaking:
       voltage = 8_V;
       break;
@@ -39,6 +43,10 @@ void Gripper::SetOutaking(double speed) {
   _state = GripperState::kOutaking;
 }
 
+void Gripper::SetStop() {
+  _state = GripperState::kStop;
+}
+
 void Gripper::SetHolding() {
   _state = GripperState::kHolding;
 }
@@ -57,5 +65,8 @@ std::string Gripper::GetState() {
   case GripperState::kOutaking:
     return "Outaking";
     break;
+  case GripperState::kStop:
+    return "Stop";
+    break;
   }
 }

src/main/cpp/Robot.cpp

@@ -29,15 +29,20 @@ void Robot::RobotInit() {
   lastPeriodic = wom::now();
 
   //sets the offsets of the swerve cancoders to retune these values set the offsets to 0 and record the values
-  map.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(4.11207_rad);
-  map.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(1.32638_rad);
-  map.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(1.67817_rad);
-  map.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0.60899_rad);
-
-  // map.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0_rad);
-  // map.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(0_rad);
-  // map.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad);
-  // map.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad);
+  map.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(2.54181_rad);
+  map.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(6.03621_rad);
+  map.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0.09050_rad);
+  map.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(5.30451_rad);
+
+    // back left: 0.090504866485253
+    // back right: 5.304505564508547
+    // front left: 2.541806165526507
+    // front right: 6.036214400329998
+
+//   map.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0_rad);
+//   map.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(0_rad);
+//   map.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad);
+//   map.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad);
 
   //the start of selecting which auto mode, was never successfully implemented 
   m_chooser.SetDefaultOption(kLowPlace, kLowPlace);

src/main/cpp/behaviour/ArmavatorBehaviour.cpp

@@ -176,35 +176,7 @@ void ArmavatorManualBehaviour::OnTick(units::second_t dt) {
 
         frc::SmartDashboard::PutNumber("ArmVelocitySetpoint", av.angleSpeed.value());
 
-        if (_codriver.GetPOV() == 0) {
-          //picking up cone down 
-
-          SetPosition(0_deg, 0.7_m, "1", 0.5, 0.2);
-        } else if (_codriver.GetPOV() == 90) {
-          //picking up cone up 
-
-          SetPosition(37.4_deg, 0.01_m, "2", 0.5, 0.2);
-        } else if (_codriver.GetPOV() == 180) {
-          //picking up cone down to collect 
-
-          SetPosition(-6_deg, 0.7_m, "3", 0.3, 0.1);
-        } else if (_codriver.GetPOV() == 270) {
-          //low hold
-
-          SetPosition(60_deg, 0.1_m, "4", 0.35, 0.07);
-        } else if (_codriver.GetXButton()) {
-          //front mid place 
-
-          SetPosition(30_deg, 0.15_m, "5", 0.35, 0.07);
-        } else if (_codriver.GetYButton()) {
-          // 152_deg 0.1814_m back high place 
-
-          SetPosition(152_deg, 0.1814_m, "6", 0.35, 0.07);
-        } else if (_codriver.GetBButton()) {
-          // high hold
-
-          SetPosition(161_deg, 0.0_m, "7", 0.35, 0.07);
-        }
+        CheckSetpoints();
         std::cout << "Mode Velocity" << std::endl;
 
       }
@@ -309,38 +281,7 @@ void ArmavatorManualBehaviour::OnTick(units::second_t dt) {
         std::cout << "set position height: " << _manualSetpoint.height.value() << std::endl;
         std::cout << "set position angle: " << _manualSetpoint.angle.value() << std::endl;
 
-        if (_codriver.GetPOV() == 0) {
-          //picking up cone down 
-
-          SetPosition(0_deg, 0.7_m, "1", 0.5, 0.2);
-        } else if (_codriver.GetPOV() == 90) {
-          //picking up cone up 
-
-          SetPosition(37.4_deg, 0.01_m, "2", 0.5, 0.2);
-        } else if (_codriver.GetPOV() == 180) {
-          //picking up cone down to collect 
-
-          SetPosition(-6_deg, 0.7_m, "3", 0.3, 0.1);
-        } else if (_codriver.GetPOV() == 270) {
-          //low hold
-
-          SetPosition(60_deg, 0.1_m, "4", 0.35, 0.07);
-        } else if (_codriver.GetXButton()) {
-          //front mid place 
-
-          SetPosition(30_deg, 0.15_m, "5", 0.35, 0.07);
-        } else if (_codriver.GetYButton()) {
-          // 152_deg 0.1814_m back high place 
-
-          SetPosition(152_deg, 0.1814_m, "6", 0.35, 0.07);
-        } else if (_codriver.GetBButton()) {
-          // high hold
-
-          SetPosition(161_deg, 0.0_m, "7", 0.35, 0.07);
-
-          std::cout << "Mode Position" << std::endl;
-
-        }
+        CheckSetpoints();
 
         break;
       }
@@ -484,11 +425,58 @@ void ArmavatorManualBehaviour::OnTick(units::second_t dt) {
   //   }
 }
 
+void ArmavatorManualBehaviour::CheckSetpoints() {
+
+  if (_codriver.GetPOV() == 0) {
+    //picking up cone down
+
+    SetPosition(0_deg, 0.7_m, "1", 0.5, 0.2);
+  } else if (_codriver.GetPOV() == 90) {
+    //picking up cone up
+
+    SetPosition(37.4_deg, 0.01_m, "2", 0.5, 0.2);
+  } else if (_codriver.GetPOV() == 180) {
+    //picking up cone down to collect
+
+    SetPosition(-6_deg, 0.7_m, "3", 0.3, 0.1);
+  } else if (_codriver.GetPOV() == 270) {
+    //low hold
+
+    SetPosition(60_deg, 0.1_m, "4", 0.35, 0.07);
+  } else if (_codriver.GetXButton()) {
+    //front mid place
+
+    SetPosition(30_deg, 0.15_m, "5", 0.35, 0.07);
+  } else if (_codriver.GetYButton()) {
+    // 152_deg 0.1814_m back high place
+
+    SetPosition(152_deg, 0.1814_m, "6", 0.35, 0.07);
+  } else if (_codriver.GetBButton()) {
+    // high hold
+
+    SetPosition(161_deg, 0.0_m, "7", 0.35, 0.07);
+
+    std::cout << "Mode Position" << std::endl;
+  }
+
+  return;
+}
+
 void ArmavatorManualBehaviour::SetPosition(units::degree_t angle, units::meter_t height, std::string name, double elevatorSpeed, double armSpeed) {
-  _setpointValue.height = height;
-  _setpointValue.angle = angle;
-  _armavator->SetPosition(_setpointValue);
-  _manualSetpoint = {_armavator->GetCurrentPosition().height, _armavator->GetCurrentPosition().angle};
-  std::cout << "GO TO armavator POS " << name << std::endl;
-  _armavator->SetSpeedValues(elevatorSpeed, armSpeed);
+  if (_armManualModes == ArmavatorManualModesEnum::kPosition) {
+    _manualSetpoint.height = height;
+    _manualSetpoint.angle  = angle;
+    //  _armavator->SetPosition(_manualSetpoint);
+    //  _manualSetpoint = {_armavator->GetCurrentPosition().height, _armavator->GetCurrentPosition().angle};
+    std::cout << "GO TO armavator POS " << name << std::endl;
+    _armavator->SetSpeedValues(elevatorSpeed, armSpeed);
+  }
+  else if (_armManualModes == ArmavatorManualModesEnum::kVelocity) {
+    ArmavatorVelocity av;
+    av.angleSpeed = angle * (180_deg / 1_s);
+    av.elevatorSpeed = height * (2_m / 1_s);
+    _armavator->SetVelocity(av);
+    frc::SmartDashboard::PutNumber("ArmVelocitySetpoint", av.angleSpeed.value());
+  }
 }
+

src/main/cpp/behaviour/GripperBehaviour.cpp

@@ -25,10 +25,20 @@ GripperBehaviour::GripperBehaviour(Gripper *gripper, frc::XboxController &codriv
 void GripperBehaviour::OnTick(units::second_t dt) {
   //setting the gripper mode
   if (_codriver.GetLeftTriggerAxis() > 0.2) {
-    gripper->SetOutaking(_codriver.GetLeftTriggerAxis());
+    double outtakeAmount = _codriver.GetLeftTriggerAxis() * _codriver.GetLeftTriggerAxis() * _codriver.GetLeftTriggerAxis();
+    gripper->SetOutaking(outtakeAmount);
   } else if (_codriver.GetRightTriggerAxis() > 0.2) {
     gripper->SetIntaking();
   } else {
-    gripper->SetIdle();
+    if (idleHold) {
+      gripper->SetIdle();
+    }
+    else {
+      gripper->SetStop();
+    }
+  }
+
+  if (_codriver.GetBackButtonPressed()) {
+    idleHold = !idleHold;
   }
 }

src/main/cpp/behaviour/SwervebaseBehaviour.cpp

@@ -74,10 +74,16 @@ void ManualDrivebase::OnTick(units::second_t deltaTime) {
   CalculateRequestedAngle(turnX, turnY, currentAngle);
   _swerveDriveTable->GetEntry("RotateMatch").SetDouble(_requestedAngle.value());
 
-  _swerveDrivebase->RotateMatchJoystick(_requestedAngle, wom::FieldRelativeSpeeds{
-    xVelocity * maxMovementMagnitude,
-    yVelocity * maxMovementMagnitude,
-    r_x * maxRotationMagnitude
+//  _swerveDrivebase->RotateMatchJoystick(_requestedAngle, wom::FieldRelativeSpeeds{
+//    xVelocity * maxMovementMagnitude,
+//    yVelocity * maxMovementMagnitude,
+//    r_x * maxRotationMagnitude
+//  });
+
+  _swerveDrivebase->SetFieldRelativeVelocity(wom::FieldRelativeSpeeds{
+      xVelocity * maxMovementMagnitude,
+      yVelocity * maxMovementMagnitude,
+      r_x * maxRotationMagnitude
   });
 }
 

src/main/include/Gripper.h

@@ -17,7 +17,8 @@ enum class GripperState {
   kIdle,
   kIntaking,
   kOutaking,
-  kHolding
+  kHolding,
+  kStop
 };
 
 class Gripper : public behaviour::HasBehaviour {
@@ -30,6 +31,7 @@ class Gripper : public behaviour::HasBehaviour {
   void SetIntaking();
   void SetOutaking(double speed);
   void SetHolding();
+  void SetStop();
 
   std::string GetState();
 

src/main/include/behaviour/ArmavatorBehaviour.h

@@ -107,4 +107,5 @@ class ArmavatorManualBehaviour : public behaviour::Behaviour {
   ArmavatorManualModeEnum _armManualModes = ArmavatorManualModeEnum::kRaw;
 
   void SetPosition(units::degree_t angle, units::meter_t height, std::string name, double elevatorSpeed, double armSpeed);
+  void CheckSetpoints();
 };

src/main/include/behaviour/GripperBehaviour.h

@@ -25,4 +25,6 @@ class GripperBehaviour : public behaviour::Behaviour {
   frc::XboxController &_codriver;
 
   bool holdingObject = false;
+
+  bool idleHold = false;
 };