AutoLeft

frc-2020/.vscode/settings.json

@@ -19,6 +19,64 @@
     "*.inc": "cpp",
     "new": "cpp",
     "cmath": "cpp",
-    "complex": "cpp"
+    "complex": "cpp",
+    "array": "cpp",
+    "atomic": "cpp",
+    "hash_map": "cpp",
+    "hash_set": "cpp",
+    "*.tcc": "cpp",
+    "bitset": "cpp",
+    "cctype": "cpp",
+    "chrono": "cpp",
+    "clocale": "cpp",
+    "condition_variable": "cpp",
+    "cstdarg": "cpp",
+    "cstddef": "cpp",
+    "cstdint": "cpp",
+    "cstdio": "cpp",
+    "cstdlib": "cpp",
+    "cstring": "cpp",
+    "ctime": "cpp",
+    "cwchar": "cpp",
+    "cwctype": "cpp",
+    "deque": "cpp",
+    "list": "cpp",
+    "unordered_map": "cpp",
+    "unordered_set": "cpp",
+    "vector": "cpp",
+    "exception": "cpp",
+    "algorithm": "cpp",
+    "functional": "cpp",
+    "iterator": "cpp",
+    "map": "cpp",
+    "memory": "cpp",
+    "memory_resource": "cpp",
+    "numeric": "cpp",
+    "optional": "cpp",
+    "random": "cpp",
+    "ratio": "cpp",
+    "set": "cpp",
+    "string": "cpp",
+    "string_view": "cpp",
+    "system_error": "cpp",
+    "tuple": "cpp",
+    "type_traits": "cpp",
+    "utility": "cpp",
+    "fstream": "cpp",
+    "future": "cpp",
+    "initializer_list": "cpp",
+    "iomanip": "cpp",
+    "iosfwd": "cpp",
+    "iostream": "cpp",
+    "istream": "cpp",
+    "limits": "cpp",
+    "mutex": "cpp",
+    "ostream": "cpp",
+    "sstream": "cpp",
+    "stdexcept": "cpp",
+    "streambuf": "cpp",
+    "thread": "cpp",
+    "cinttypes": "cpp",
+    "typeinfo": "cpp"
   }
 }

frc-2020/src/main/cpp/RobotContainer.cpp

@@ -40,14 +40,15 @@
 
 
 #include "commands/Autos/AutoLeftCommandGroup.h"
-// #include "commands/Autos/AutoRightCommandGroup.h"
+// #include "commands/Autos/AutoLeftCommandGroupStrafe.h"
 // #include "commands/Autos/AutoTestCommandGroup.h"
-
+#include "commands/Autos/FordAuto.h"
 
 #include <frc/geometry/Pose2d.h>
 #include <frc/geometry/Translation2d.h>
 #include <frc/smartdashboard/SmartDashboard.h>
-#include <units/units.h>
+#include <units/angle.h>
+#include <units/velocity.h>
 #include "Constants.h"
 #include <frc2/command/WaitCommand.h>
 
@@ -80,13 +81,41 @@
 
 
 
+#include "Paths.h"
 using namespace DriveConstants;
 
 RobotContainer::RobotContainer(): m_driverController(0),
                                   m_auxController(1),
                                   m_tDpadAux(&m_auxController, XBOX_DPAD_TOP),
                                   m_bDpadAux(&m_auxController, XBOX_DPAD_BOTTOM){
-  ConfigureButtonBindings();
+    ConfigureButtonBindings();
+    double metersToInches = 39.87;
+  std::vector<SwerveDrivePathGenerator::waypoint_t> one;
+  one.push_back(SwerveDrivePathGenerator::waypoint_t {RobotParameters::k_robotLength/2.0, 120 + (metersToInches*RobotParameters::k_wheelBase)/2.0, 90, 0, 0});//start
+  one.push_back(SwerveDrivePathGenerator::waypoint_t {56, 206, 21.57, 1000, 10000});
+  one.push_back(SwerveDrivePathGenerator::waypoint_t {103.315+30.76, 246.906-8.805+6, 21.57, 0, 0});//pick up ball 4 and 5
+  m_followerLeft[0].generatePath(one, "one");//247  147
+
+  std::vector<SwerveDrivePathGenerator::waypoint_t> two;
+  two.push_back(SwerveDrivePathGenerator::waypoint_t {103.315+30.76, 246.906-8.805+6, 21.57, 0, 0});//pick up ball 4 and 5
+  two.push_back(SwerveDrivePathGenerator::waypoint_t {101.67+2.39-2.868-12, 231.5-6.577-0.877-3.5, 90, RobotParameters::k_maxSpeed*39.38, 0});//move
+  two.push_back(SwerveDrivePathGenerator::waypoint_t {77.2, 231.5-6.577-0.877-7.01+2.86+2.86, 180, 0, 0});//move to shoot
+  m_followerLeft[1].generatePath(two, "two");//92 234
+
+  std::vector<SwerveDrivePathGenerator::waypoint_t> three;
+  three.push_back(SwerveDrivePathGenerator::waypoint_t {77.2, 231.5-6.577-0.877-7.01+2.86+2.86, 188, 0, 0});//shoot pose
+  three.push_back(SwerveDrivePathGenerator::waypoint_t {36, 189, 90, (RobotParameters::k_maxSpeed)*39.38, 0}); // move
+  three.push_back(SwerveDrivePathGenerator::waypoint_t {32, 320, 90, 0, 0}); // pick up ball 6 - 8
+  m_followerLeft[2].generatePath(three, "three");
+
+
+  std::vector<SwerveDrivePathGenerator::waypoint_t> four;
+  four.push_back(SwerveDrivePathGenerator::waypoint_t {32, 320, 90, 0, 0}); // move
+  four.push_back(SwerveDrivePathGenerator::waypoint_t {54, 249, 130, (RobotParameters::k_maxSpeed)*39.38, 0}); // move
+  four.push_back(SwerveDrivePathGenerator::waypoint_t {77, 240, 187, 0, 0}); // move to shoot
+  m_followerLeft[3].generatePath(four, "four");
+
+
   // frc::SmartDashboard::PutData("swerve drive sub", &m_drive);
   frc::SmartDashboard::PutData("rotate to angle", new RotateWithTrajectoryCommand(&m_drive,frc::SmartDashboard::GetNumber("angle to rotate", 0),.55));
 
@@ -106,8 +135,8 @@ RobotContainer::RobotContainer(): m_driverController(0),
   frc::SmartDashboard::PutNumber("limeLight Izone", RobotParameters::k_limeLightIZone);
 
   frc::SmartDashboard::PutNumber("Lime light X", 0);
-    frc::SmartDashboard::PutNumber("Lime light Y", 0);
-    frc::SmartDashboard::PutNumber("Lime light yaw", 0);
+  frc::SmartDashboard::PutNumber("Lime light Y", 0);
+  frc::SmartDashboard::PutNumber("Lime light yaw", 0);
   frc::SmartDashboard::PutData("limelight to angle", new LimeLightRotateToTargetCommand(&m_drive,.01));
   frc::SmartDashboard::PutNumber("motion magic target angle", 0);
   frc::SmartDashboard::PutData("motion magic to angle", new RotateWithMotionMagic(&m_drive, 20,1,true));
@@ -161,8 +190,8 @@ RobotContainer::RobotContainer(): m_driverController(0),
     m_climber.zeroPos();
   },{&m_climber}));
 
-   frc::SmartDashboard::PutNumber("climberSetPos", 0);
-   frc::SmartDashboard::PutData("climb to pos", new frc2::InstantCommand([this]{
+  frc::SmartDashboard::PutNumber("climberSetPos", 0);
+  frc::SmartDashboard::PutData("climb to pos", new frc2::InstantCommand([this]{
     m_climber.goToPos(frc::SmartDashboard::GetNumber("climberSetPos", 0));
   },{&m_climber}));
 
@@ -256,12 +285,14 @@ void RobotContainer::ConfigureButtonBindings(){
 frc2::Command* RobotContainer::GetAutonomousCommand() {
 
   // return nullptr;
- return new AutoLeftCommandGroup(&m_follower, &m_drive, &m_shooter, &m_feeder, &m_intake);
+  // return new FordAuto(&m_follower, &m_drive, &m_shooter, &m_feeder, &m_intake);
+  return new AutoLeftCommandGroup(&m_drive, &m_shooter, &m_feeder, &m_intake, m_followerLeft);
+  // return new AutoTestCommandGroup(&m_follower, &m_drive, &m_shooter, &m_feeder, &m_intake); //
 }
 
 frc2::InstantCommand* RobotContainer::GetBrakeCommand(){
   return new frc2::InstantCommand([this]{m_drive.setBrake(); TurnLimeLightOff();},{&m_drive});
 }
-  frc2::InstantCommand* RobotContainer::GetCoastCommand(){
-    return new frc2::InstantCommand([this]{m_drive.setCoast();},{&m_drive});
-  }
+frc2::InstantCommand* RobotContainer::GetCoastCommand(){
+  return new frc2::InstantCommand([this]{m_drive.setCoast();},{&m_drive});
+}

frc-2020/src/main/cpp/Utils/SwerveDrivePathFollower.cpp

@@ -105,16 +105,13 @@ void SwerveDrivePathFollower::Update(frc::Pose2d pose){
 		frc::SmartDashboard::PutNumber("pathSpeed", m_path[m_pathIndex].vel);
 		m_time += 1.0/RobotParameters::k_updateRate;
 
-		double robotYawRate = m_path[m_pathIndex].yawRate;
 		// double robotXVel = m_path[m_pathIndex].xVel;
 		// double robotYVel = m_path[m_pathIndex].yVel;
 		double robotVel = m_path[m_pathIndex].vel;
 		double poseX = m_path[m_lookAheadIndex].xPos;
 		double poseY = m_path[m_lookAheadIndex].yPos;
 		double followerYaw = m_path[m_pathIndex].yaw;
 		double followerYawRate = m_path[m_pathIndex].yawRate;
-		double closestX = m_path[m_pathIndex].xPos;
-		double closestY = m_path[m_pathIndex].yPos;
 		//calculating new vector
 		double distX = poseX - pose.Translation().X().to<double>();
 		double distY = poseY - pose.Translation().Y().to<double>();
@@ -130,29 +127,6 @@ void SwerveDrivePathFollower::Update(frc::Pose2d pose){
 		// printf("path x vel update method %f\n", m_xVel);
 		frc::SmartDashboard::PutNumber("x follower pos", poseX);
 		frc::SmartDashboard::PutNumber("y follower pos", poseY);	
-
-				double actualYaw = pose.Rotation().Degrees().to<double>();
-		// m_File << m_path[m_pathIndex].time << ",";
-		// m_File << closestX << ",";
-		// m_File << closestY << ",";
-		// m_File << robotVel << ",";
-		// m_File << followerYaw << ",";
-		// m_File << robotYawRate << ",";//6
-
-
-		// m_File << pose.Translation().X().to<double>() << ",";
-		// m_File << pose.Translation().Y().to<double>() << ",";
-		// m_File << actualYaw << ",";
-		// m_File << skipped << ",";//4
-
-
-		// //look ahead info
-		// m_File << m_path[m_lookAheadIndex].xPos << ",";
-		// m_File << m_path[m_lookAheadIndex].yPos << ",";
-		// m_File << vectorAngle*180/MATH_CONSTANTS_PI << ",";
-		// m_File << cos(vectorAngle)*robotVel << ",";
-		// m_File << sin(vectorAngle)*robotVel << ",";//5	
-		// m_File << m_lookAhead <<"\n";
 }
 
 
@@ -217,6 +191,9 @@ double SwerveDrivePathFollower::getYawRate(){
 frc::Pose2d SwerveDrivePathFollower::getPointPos(int index){
 	return frc::Pose2d(units::meter_t(m_path[index].xPos),units::meter_t(m_path[index].yPos), frc::Rotation2d(units::degree_t(m_path[index].yaw)));
 }
+frc::Pose2d SwerveDrivePathFollower::getFollowerPos(){
+	return frc::Pose2d(units::meter_t(m_path[m_pathIndex].xPos),units::meter_t(m_path[m_pathIndex].yPos), frc::Rotation2d(units::degree_t(m_path[m_pathIndex].yaw)));
+}
 void SwerveDrivePathFollower::stop(bool interrupted){
     // m_File.close();
     frc::SmartDashboard::PutBoolean("auto interrupted", interrupted);

frc-2020/src/main/cpp/subsystems/DriveSubsystem.cpp

@@ -8,7 +8,8 @@
 #include "subsystems/DriveSubsystem.h"
 
 #include <frc/geometry/Rotation2d.h>
-#include <units/units.h>
+#include <units/angle.h>
+#include <units/velocity.h>
 
 #include "Constants.h"
 #include <frc/smartdashboard/SmartDashboard.h>

frc-2020/src/main/cpp/subsystems/IntakeSubsystem.cpp

@@ -19,13 +19,13 @@
     }
 
     void IntakeSubsystem::extend(){
-        printf("----------extend----------\n");
+        // printf("----------extend----------\n");
         m_intakeExtended = true;
         m_intakeSolenoid.Set(m_intakeSolenoid.kForward);//frc::DoubleSolenoid::Value::kReverse);//TODO uncomment
     }
     void IntakeSubsystem::retract(){
         m_intakeExtended = false;
-        printf("----------retract----------\n");
+        // printf("----------retract----------\n");
         m_intakeSolenoid.Set(m_intakeSolenoid.kReverse);//Set(frc::DoubleSolenoid::Value::kForward);//TODO uncomment
     }
     bool IntakeSubsystem::isIntakeExtended(){

frc-2020/src/main/include/Constants.h

@@ -8,9 +8,9 @@
 #include <frc/geometry/Translation2d.h>
 #include <frc/kinematics/SwerveDriveKinematics.h>
 #include <frc/trajectory/TrapezoidProfile.h>
-#include <units/units.h>
+#include <units/velocity.h>
 #include <wpi/math>
-
+#include "RobotParameters.h"
 #pragma once
 
 /**
@@ -103,9 +103,8 @@ using radians_per_second_squared_t =
     units::compound_unit<units::radians,
                          units::inverse<units::squared<units::second>>>;
 
-constexpr auto kMaxSpeed = units::meters_per_second_t(3);
-constexpr auto kMaxAcceleration = units::meters_per_second_squared_t(3);
-constexpr auto kMaxAngularSpeed = units::radians_per_second_t(3.142);
+constexpr auto kMaxSpeed = units::meters_per_second_t(RobotParameters::k_maxSpeed);
+constexpr auto kMaxAngularSpeed = units::degrees_per_second_t(RobotParameters::k_maxYawRate);
 constexpr auto kMaxAngularAcceleration =
     units::unit_t<radians_per_second_squared_t>(3.142);
 
@@ -153,7 +152,7 @@ namespace IndexerConstants{
 // static constexpr double test = -(1 -.059) /(1-.38) +1;
 namespace FeederConstants{
     static constexpr double kDefaultFeederSpeed = .8;
-    static constexpr double kIntakeBallDistance = 30;//28 - 35 tested
+    static constexpr double kIntakeBallDistance = 28;//28 - 35 tested was 30
     static constexpr double kLastBallDistance = 26;//28 - 35 tested
     static constexpr double kShootingFarFeederSpeed = .5;//28 - 35 tested
     static constexpr double kShootingFeederSpeed = 1;//28 - 35 tested

frc-2020/src/main/include/RobotContainer.h

@@ -51,10 +51,15 @@ class RobotContainer {
   // PanelManipulatorSubsystem m_colorSensor;
   // PanelManipulatorSubsystem::Color_t m_color;
   FeederSubsystem m_feeder;
-  SwerveDrivePathFollower m_follower;
+
   ClimberSubsystem m_climber;
   IndexerSubsystem m_indexer;
 
+
+  SwerveDrivePathFollower m_followerLeft[4];
+  // SwerveDrivePathFollower m_followerLeftOne;
+
+
   frc2::Button m_startDriver{[&] { return m_driverController.GetRawButton(XBOX_START_BUTTON); }};//
   frc2::Button m_backDriver{[&] { return m_driverController.GetRawButton(XBOX_BACK_BUTTON); }};//
 

frc-2020/src/main/include/Utils/CoordinateTranslation.h

@@ -17,14 +17,14 @@ class CoordinateTranslation {
   }
 
   static void NolanToWPILib(std::vector<SwerveDrivePathGenerator::finalPathPoint_t> nolanPath,std::vector<SwerveDrivePathGenerator::finalPathPoint_t> &wpiPath){
-    std::ofstream m_File;
-    std::remove("home/lvuser/NolanToWPILib.csv");
-		m_File.open("home/lvuser/NolanToWPILib.csv");
-		m_File <<"xPos (m), yPos (m), yaw (deg), vel (m/s),   heading, accel(m/s/s),  dist(m)\n";
+    // std::ofstream m_File;
+    // std::remove("home/lvuser/NolanToWPILib.csv");
+		// m_File.open("home/lvuser/NolanToWPILib.csv");
+		// m_File <<"xPos (m), yPos (m), yaw (deg), vel (m/s),   heading, accel(m/s/s),  dist(m)\n";
     wpiPath.clear();
     double inchesToMeters = .0254;
     for(int i = 0; i < (int)nolanPath.size();i++){
-      printf("NolanToWPILib cycle: %d\n", i);
+      // printf("NolanToWPILib cycle: %d\n", i);
       SwerveDrivePathGenerator::finalPathPoint_t wpiPoint = nolanPath[i];
       wpiPoint.xPos = nolanPath[i].yPos*inchesToMeters;
       wpiPoint.yPos = -nolanPath[i].xPos*inchesToMeters;
@@ -34,40 +34,40 @@ class CoordinateTranslation {
       wpiPoint.accel = nolanPath[i].accel*inchesToMeters;
       wpiPoint.dist = nolanPath[i].dist*inchesToMeters;
       wpiPath.push_back(wpiPoint);
-      m_File << wpiPoint.xPos << ", ";
-      m_File << wpiPoint.yPos << ", ";
-      m_File << wpiPoint.yaw << ", ";
-      m_File << wpiPoint.vel << ", ";
-      m_File << wpiPoint.heading << ", ";
-      m_File << wpiPoint.accel << ", ";
-      m_File << wpiPoint.dist << ", ";
-      m_File << "\n";
-      m_File.close();
+      // m_File << wpiPoint.xPos << ", ";
+      // m_File << wpiPoint.yPos << ", ";
+      // m_File << wpiPoint.yaw << ", ";
+      // m_File << wpiPoint.vel << ", ";
+      // m_File << wpiPoint.heading << ", ";
+      // m_File << wpiPoint.accel << ", ";
+      // m_File << wpiPoint.dist << ", ";
+      // m_File << "\n";
+      // m_File.close();
     }
   }
   static void WPILibToNolan(std::vector<SwerveDrivePathGenerator::waypoint_t> wpiPath, std::vector<SwerveDrivePathGenerator::waypoint_t> &nolanPath){
     double metersToInches = 39.3701;
     // double metersToInches = 12.0;// inches to feet
-    std::ofstream m_File;
-    std::remove("home/lvuser/WPILibToNolan.csv");
-		m_File.open("home/lvuser/WPILibToNolan.csv");
-		m_File <<"xPos (m), yPos (m), yaw (deg), speed (m/s),   radCurve\n";
-    nolanPath.clear();
+    // std::ofstream m_File;
+    // std::remove("home/lvuser/WPILibToNolan.csv");
+		// m_File.open("home/lvuser/WPILibToNolan.csv");
+		// m_File <<"xPos (m), yPos (m), yaw (deg), speed (m/s),   radCurve\n";
+    // nolanPath.clear();
     for(int i = 0; i < (int)wpiPath.size();i++){
-      printf("WPILibToNolan cycle: %d\n", i);
+      // printf("WPILibToNolan cycle: %d\n", i);
       SwerveDrivePathGenerator::waypoint_t nolanPoint = wpiPath[i];
       nolanPoint.xPos = -wpiPath[i].yPos*metersToInches;
       nolanPoint.yPos = wpiPath[i].xPos*metersToInches;
       nolanPoint.speed = wpiPath[i].speed*metersToInches;
       nolanPoint.radCurve = wpiPath[i].radCurve*metersToInches;
       nolanPoint.yaw = normalizeToRange::NormalizeToRange(wpiPath[i].yaw+90,-180,180, true);
       nolanPath.push_back(nolanPoint);
-      m_File << nolanPoint.xPos << ", ";
-      m_File << nolanPoint.yPos << ", ";
-      m_File << nolanPoint.yaw << ", ";
-      m_File << nolanPoint.speed << ", ";
-      m_File << nolanPoint.radCurve << ", ";
-      m_File << "\n";
+      // m_File << nolanPoint.xPos << ", ";
+      // m_File << nolanPoint.yPos << ", ";
+      // m_File << nolanPoint.yaw << ", ";
+      // m_File << nolanPoint.speed << ", ";
+      // m_File << nolanPoint.radCurve << ", ";
+      // m_File << "\n";
     }
   } 
   private:

frc-2020/src/main/include/Utils/SwerveDrivePathFollower.h

@@ -46,6 +46,7 @@ class SwerveDrivePathFollower {
   int lookAheadIndex();
   bool isPathFinished();
   frc::Pose2d getPointPos(int index);
+  frc::Pose2d getFollowerPos();
   // bool targetPosFound();
   // void setTargetPos();
   void stop(bool interrupted);