Merge pull request #529 from Coolearthsky/demoBot

Work  on demo bot

lib/src/main/java/org/team100/lib/config/Feedforward100.java

@@ -53,7 +53,7 @@ public static Feedforward100 makeNeoArm() {
 
     // TODO get the acurate numbers here
     public static Feedforward100 makeNeo550() {
-        return new Feedforward100(0.32, 0, 0, .07, 0);
+        return new Feedforward100(0.12, 0, 0, .07, 0);
     }
 
     public static Feedforward100 makeArmPivot() {

studies/demoBot/src/main/java/org/team100/frc2024/RobotContainer.java

@@ -7,15 +7,13 @@
 import org.team100.frc2024.drivetrain.TankModuleCollection;
 import org.team100.frc2024.drivetrain.commands.DriveManually;
 import org.team100.frc2024.shooter.commands.Shoot;
-import org.team100.frc2024.shooter.commands.ShootOne;
 import org.team100.frc2024.shooter.drumShooter.DrumShooter;
 import org.team100.frc2024.shooter.drumShooter.ShooterCollection;
 import org.team100.frc2024.shooter.indexer.Indexer;
 import org.team100.frc2024.shooter.indexer.IndexerCollection;
 import org.team100.frc2024.shooter.pivot.PivotCollection;
 import org.team100.frc2024.shooter.pivot.PivotSubsystem;
 import org.team100.frc2024.shooter.pivot.commands.PivotDefault;
-import org.team100.frc2024.shooter.pivot.commands.ZeroPivot;
 import org.team100.lib.async.Async;
 import org.team100.lib.async.AsyncFactory;
 import org.team100.lib.framework.TimedRobot100;
@@ -54,19 +52,19 @@ public RobotContainer(TimedRobot100 robot) throws IOException {
 
         final LoggerFactory sysLog = logger.child("Subsystems");
 
-        m_drive = new TankDriveSubsystem(fieldLogger, TankModuleCollection.get(fieldLogger, 40));
+        m_drive = new TankDriveSubsystem(fieldLogger, TankModuleCollection.get(fieldLogger, 20));
         m_drive.setDefaultCommand(new DriveManually(driverControl::velocity, m_drive));
 
         m_shooter = new DrumShooter(sysLog, ShooterCollection.get(sysLog, 20));
         m_shooter.setDefaultCommand(m_shooter.run(m_shooter::stop));
-
-        m_pivot = new PivotSubsystem(sysLog, PivotCollection.get(sysLog, 20));
+        
+        m_pivot = new PivotSubsystem(sysLog, PivotCollection.get(sysLog, 15));
         m_pivot.setDefaultCommand(new PivotDefault(driverControl::shooterPivot, m_pivot));
 
         m_indexer = IndexerCollection.get(sysLog);
 
         whileTrue(driverControl::ampLock, new Shoot(m_shooter, m_indexer));
-        whileTrue(driverControl::fullCycle, new ShootOne(m_shooter, m_indexer));
+        // whileTrue(driverControl::fullCycle, new ShootOne(m_shooter, m_indexer));
         whileTrue(driverControl::driveToNote, m_shooter.run(m_shooter::spinUp));
 
         m_auton = null;
@@ -76,7 +74,8 @@ public void onInit() {
     }
 
     public void onTeleopInit() {
-        new ZeroPivot(m_pivot).schedule();
+        m_pivot.setEncoderPosition(Math.PI/2);
+        // new ZeroPivot(m_pivot).schedule();
     }
 
     public void periodic() {

studies/demoBot/src/main/java/org/team100/frc2024/drivetrain/TankModuleCollection.java

@@ -13,7 +13,7 @@ public class TankModuleCollection {
     private static final String kSwerveModules = "Tank Modules";
     private static final String kLeft = "Left";
     private static final String kRight = "Right";
-    private static final double m_gearRatio = 5.2307692308;
+    private static final double m_5to1 = 5.2307692308;
     private static final double m_wheelDiameter = 0.098425;
 
     private final OutboardLinearVelocityServo m_rightDrive;
@@ -35,9 +35,9 @@ public static TankModuleCollection get(
             case DEMO_BOT:
                 Util.println("************** Custom Tank Module Modules, using in built encoders? **************");
                 OutboardLinearVelocityServo rightMotor = new OutboardLinearVelocityServo(collectionLogger, Neo550Factory.getNEO550LinearMechanism(
-                        kRight, collectionLogger, currentLimit, 3, Math.pow(m_gearRatio, 2), MotorPhase.REVERSE, m_wheelDiameter));
+                        kRight, collectionLogger, currentLimit, 2, Math.pow(m_5to1, 2), MotorPhase.REVERSE, m_wheelDiameter));
                 OutboardLinearVelocityServo leftMotor = new OutboardLinearVelocityServo(collectionLogger, Neo550Factory.getNEO550LinearMechanism(
-                        kLeft, collectionLogger, currentLimit, 2, Math.pow(m_gearRatio, 2), MotorPhase.FORWARD, m_wheelDiameter));
+                        kLeft, collectionLogger, currentLimit, 3, Math.pow(m_5to1, 2), MotorPhase.FORWARD, m_wheelDiameter));
                 return new TankModuleCollection(leftMotor, rightMotor);
             case BLANK:
             Util.println("************** SIMULATED MODULES **************");

studies/demoBot/src/main/java/org/team100/frc2024/drivetrain/commands/DriveManually.java

@@ -57,7 +57,7 @@ public DriveManually(
             "Manual",
                 new TankDriver() {
                     public void apply(DriverControl.Velocity t) {
-                        m_drive.set(t.x(), t.theta());
+                        m_drive.set(t.x(), t.y());
                     }
         });
     }

studies/demoBot/src/main/java/org/team100/frc2024/shooter/commands/Shoot.java

@@ -24,7 +24,9 @@ public void initialize() {
 
     @Override
     public void execute() {
-        m_indexer.set(1);
+        if (m_shooter.atVeloctity()) {
+            m_indexer.set(1);
+        }
     }
 
     @Override

studies/demoBot/src/main/java/org/team100/frc2024/shooter/commands/ShootOne.java

@@ -23,13 +23,12 @@ public ShootOne(
     @Override
     public void initialize() {
         m_shooter.spinUp();
-        angle = m_indexer.getAngle();
     }
 
     @Override
     public void execute() {
         if (m_shooter.atVeloctity()) {
-            m_indexer.setAngle(angle + distanceDeg);
+            m_indexer.set(angle + distanceDeg);
         }
     }
 
@@ -38,8 +37,8 @@ public void end(boolean interrupted) {
         m_shooter.stop();
     }
 
-    @Override
-    public boolean isFinished() {
-        return Math.abs(m_indexer.getAngle() - (angle + distanceDeg)) < 0.05;
-    }
+    // @Override
+    // public boolean isFinished() {
+        // return Math.abs(m_indexer.getAngle() - (angle + distanceDeg)) < 0.05;
+    // }
 }

studies/demoBot/src/main/java/org/team100/frc2024/shooter/drumShooter/DrumShooter.java

@@ -30,7 +30,7 @@ public class DrumShooter extends SubsystemBase implements Glassy {
 
     private final LinearVelocityServo m_leftRoller;
     private final LinearVelocityServo m_rightRoller;
-    private final double shooterVelocityM_S = 30;
+    private final double shooterVelocityM_S = 10;
 
     private double currentDesiredLeftVelocity = 0;
     private double currentDesiredRightVelocity = 0;
@@ -80,7 +80,7 @@ public double getVelocity() {
     }
 
     public boolean atVeloctity() {
-        return atVeloctity(0.5);
+        return atVeloctity(10);
     }
 
     /**

studies/demoBot/src/main/java/org/team100/frc2024/shooter/drumShooter/ShooterCollection.java

@@ -28,8 +28,8 @@ public static ShooterCollection get(
             switch (Identity.instance) {
                 case DEMO_BOT:
                 //TODO get the real diameter, gearRatios, and canIDs, and Indexer accel
-                LinearVelocityServo leftDrum = Neo550Factory.getNEO550VelocityServo(kLeft, collectionLogger,currentLimit,4,m_gearRatio,MotorPhase.FORWARD, shooterWheelDiameterM);
-                LinearVelocityServo rightDrum = Neo550Factory.getNEO550VelocityServo(kRight, collectionLogger,currentLimit,5,m_gearRatio,MotorPhase.REVERSE, shooterWheelDiameterM);
+                LinearVelocityServo leftDrum = Neo550Factory.getNEO550VelocityServo(kLeft, collectionLogger,currentLimit,39,m_gearRatio,MotorPhase.FORWARD, shooterWheelDiameterM);
+                LinearVelocityServo rightDrum = Neo550Factory.getNEO550VelocityServo(kRight, collectionLogger,currentLimit,19,m_gearRatio,MotorPhase.REVERSE, shooterWheelDiameterM);
                 return new ShooterCollection(leftDrum, rightDrum);
                 case BLANK:
                 default:

studies/demoBot/src/main/java/org/team100/frc2024/shooter/indexer/Indexer.java

@@ -7,10 +7,6 @@
 public interface Indexer extends Glassy, Subsystem {
 
     public void set(double value);
-
-    public void setAngle(double value);
-
-    public double getAngle();
 
     public void stop();
 } 

studies/demoBot/src/main/java/org/team100/frc2024/shooter/indexer/IndexerCollection.java

@@ -2,15 +2,16 @@
 
 import org.team100.lib.config.Identity;
 import org.team100.lib.logging.LoggerFactory;
+import org.team100.lib.motor.MotorPhase;
 
 public class IndexerCollection {
 
     public static Indexer get(
             LoggerFactory parent
             ) {
         switch (Identity.instance) {
-            case BETA_BOT:
-                return new IndexerServo(parent, 0);
+            case DEMO_BOT:
+                return new IndexerServo(parent, MotorPhase.REVERSE, 0);
             case BLANK:
             default:
                 return new Indexer() {
@@ -21,18 +22,6 @@ public void set(double value) {
                         throw new UnsupportedOperationException("Unimplemented method 'set'");
                     }
 
-                    @Override
-                    public void setAngle(double value) {
-                        // TODO Auto-generated method stub
-                        throw new UnsupportedOperationException("Unimplemented method 'setAngle'");
-                    }
-
-                    @Override
-                    public double getAngle() {
-                        // TODO Auto-generated method stub
-                        throw new UnsupportedOperationException("Unimplemented method 'getAngle'");
-                    }
-
                     @Override
                     public void stop() {
                         // TODO Auto-generated method stub

studies/demoBot/src/main/java/org/team100/frc2024/shooter/indexer/IndexerServo.java

@@ -3,47 +3,40 @@
 import org.team100.lib.logging.Level;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.LoggerFactory.DoubleLogger;
+import org.team100.lib.motor.MotorPhase;
 
-import edu.wpi.first.wpilibj.Servo;
+import edu.wpi.first.wpilibj.PWM;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class IndexerServo extends SubsystemBase implements Indexer {
 
-    private final Servo m_servo;
+    private final PWM m_servo;
     private final DoubleLogger m_doubleLogger;
+    private final int m_motorPhase;
 
-    private double speed;
-
-    public IndexerServo(LoggerFactory parent, int channel) {
+    public IndexerServo(LoggerFactory parent, MotorPhase motorPhase,int channel) {
         LoggerFactory logger = parent.child(this);
+        if (MotorPhase.FORWARD.equals(motorPhase)) {
+            m_motorPhase = 1;
+        } else {
+            m_motorPhase = -1;
+        }
         m_doubleLogger = logger.doubleLogger(Level.TRACE, "Angle (deg)");
-        m_servo = new Servo(channel);
+        m_servo = new PWM(channel);
         stop();
     }
 
+    @Override
     public void set(double value) {
-        speed = value;
+        m_servo.setSpeed(value * m_motorPhase);
+        m_doubleLogger.log(() -> value);
     } 
 
+    @Override
     public void stop() {
-        speed = 0;
-    } 
-
-    public void setAngle(double value) {
-        stop();
-        m_servo.setAngle(value);
+        m_servo.setSpeed(0);
     } 
 
-    public double getAngle() {
-        return m_servo.getAngle();
-    }
-
-    @Override
-    public void periodic() {
-        m_doubleLogger.log(() -> getAngle());
-        m_servo.setAngle(getAngle() + speed);
-    }
-
     @Override
     public String getGlassName() {
         return "Indexer";

studies/demoBot/src/main/java/org/team100/frc2024/shooter/indexer/IndexerSubsystem.java

@@ -61,12 +61,10 @@ public void set(double value) {
         m_linearMechanism.setVelocity(value,0, 0);
     }
 
-    @Override
     public void setAngle(double value) {
         m_indexer.setPosition(value,0);
     }
 
-    @Override
     public double getAngle() {
         return m_indexer.getPosition().getAsDouble();
     }

studies/demoBot/src/main/java/org/team100/frc2024/shooter/pivot/PivotCollection.java

@@ -1,17 +1,23 @@
 package org.team100.frc2024.shooter.pivot;
 
+import org.team100.lib.config.Feedforward100;
 import org.team100.lib.config.Identity;
+import org.team100.lib.config.PIDConstants;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.motion.servo.OutboardGravityServo;
 import org.team100.lib.motor.MotorPhase;
+import org.team100.lib.motor.Neo550CANSparkMotor;
+import org.team100.lib.profile.TrapezoidProfile100;
 import org.team100.lib.util.Neo550Factory;
 
 public class PivotCollection {
     private static final String kPivot = "Pivot";
 
-    private final OutboardGravityServo m_pivot;
+    private final Neo550CANSparkMotor m_pivot;
+    private static final double m_5to1 = 5.2307692308;
+    private static final double m_4to1 = 3.61;
 
-    private PivotCollection(OutboardGravityServo pivot) {
+    private PivotCollection(Neo550CANSparkMotor pivot) {
         m_pivot = pivot;
     }
 
@@ -23,15 +29,15 @@ public static PivotCollection get(
         switch (Identity.instance) {
             case DEMO_BOT:
             //TODO get canID, gearRatio, p values, and gravityNm
-            OutboardGravityServo pivot = Neo550Factory.getNEO550GravityServo(kPivot, collectionLogger, currentLimit, 1, 14, MotorPhase.FORWARD,1,0.1,0,-Math.PI/2, 0);
+            Neo550CANSparkMotor pivot = new Neo550CANSparkMotor(collectionLogger, 5, MotorPhase.FORWARD, currentLimit, Feedforward100.makeNeo550(), new PIDConstants(1));
             return new PivotCollection(pivot);
             case BLANK:
             default:
-            return new PivotCollection(Neo550Factory.simulatedGravityServo(collectionLogger));
+            throw new UnsupportedOperationException("Not correct robot");
         }
     }
 
-    public OutboardGravityServo getPivot() {
+    public Neo550CANSparkMotor getPivot() {
         return m_pivot;
     }
 }

studies/demoBot/src/main/java/org/team100/frc2024/shooter/pivot/PivotSubsystem.java

@@ -5,30 +5,39 @@
 import org.team100.lib.dashboard.Glassy;
 import org.team100.lib.logging.Level;
 import org.team100.lib.logging.LoggerFactory;
+import org.team100.lib.logging.LoggerFactory.DoubleLogger;
 import org.team100.lib.logging.LoggerFactory.OptionalDoubleLogger;
 import org.team100.lib.motion.servo.OutboardGravityServo;
+import org.team100.lib.motor.Neo550CANSparkMotor;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class PivotSubsystem extends SubsystemBase implements Glassy {
 
-    private final OutboardGravityServo m_pivot;
-    private final OptionalDoubleLogger m_logger;
-
+    private final Neo550CANSparkMotor m_pivot;
+    private final DoubleLogger m_logger;
     public PivotSubsystem(
             LoggerFactory parent,
             PivotCollection pivotCollection) {
         LoggerFactory logger = parent.child(this);
-        m_logger = logger.optionalDoubleLogger(Level.TRACE, "Pivot Position (rad)");
+        m_logger = logger.doubleLogger(Level.TRACE, "Pivot Position (rad)");
         m_pivot = pivotCollection.getPivot();
     }
 
-    public void setAngleRad(double angle) {
-        m_pivot.setPosition(angle);
+    // public void setAngleRad(double angle) {
+    //     m_pivot.setD(angle);
+    // }
+
+    public void dutyCycle(double set) {
+        m_pivot.setDutyCycle(set);
     }
 
-    public OptionalDouble getAngleRad() {
-        return m_pivot.getPositionRad();
+    // public void setVelocityRad_S(double angle_S) {
+    //     m_pivot.setVelocity(angle_S);
+    // }
+
+    public double getAngleRad() {
+        return m_pivot.getPositionRot();
     }
 
     public void setEncoderPosition(double positionRad) {

studies/demoBot/src/main/java/org/team100/frc2024/shooter/pivot/commands/PivotDefault.java

@@ -11,19 +11,20 @@ public class PivotDefault extends Command {
     private final Supplier<Double> m_twistSupplier;
     private final PivotSubsystem m_pivot;
 
-    private final double rate = 0.01;
+    private final double rate = -0.5;
 
     public PivotDefault(Supplier<Double> twistSupplier, PivotSubsystem pivot) {
         m_twistSupplier = twistSupplier;
         m_pivot = pivot;
+        addRequirements(m_pivot);
     }
 
     @Override
     public void initialize() {}
 
     @Override
     public void execute() {
-        m_pivot.setAngleRad(m_pivot.getAngleRad().getAsDouble() + m_twistSupplier.get() * rate);
+        m_pivot.dutyCycle(m_twistSupplier.get());
     }
 
     @Override