Merge branch 'master' of https://github.com/Spartronics4915/2020-Infi…

…niteRecharge

src/main/java/com/spartronics4915/frc2020/Constants.java

@@ -1,5 +1,8 @@
 package com.spartronics4915.frc2020;
 
+import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.I2C.Port;
+
 public final class Constants {
 
     public static final class Climber {
@@ -12,4 +15,19 @@ public static final class OI {
         public static final int kButtonBoardId = 1;
     }
 
+    public static final class PanelRotator {
+        public static final int kBeamSensorUpID = -1;
+        public static final int kBeamSensorDownID = -1;
+
+        public static final int kExtendMotorID = -1;
+        public static final int kSpinMotorID = -1;
+
+        //TODO:This is bad and does not work. Needs to be implemented in a functional way.
+        public static final Port kColorSensorLeftID = I2C.Port.kOnboard;
+        public static final Port kColorSensorRightID = I2C.Port.kOnboard;
+
+        public static final double kExtendMotorSpeed = 0.5;
+        public static final double kSpinMotorSpeed = 0.5;
+    }
+
 }

src/main/java/com/spartronics4915/frc2020/commands/SpinToColorCommand.java

@@ -0,0 +1,37 @@
+package com.spartronics4915.frc2020.commands;
+
+import com.spartronics4915.frc2020.subsystems.PanelRotator;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class SpinToColorCommand extends CommandBase {
+
+    // You should only use one subsystem per command. If multiple are needed, use a CommandGroup.
+    public SpinToColorCommand(PanelRotator mSubsystem) {
+
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end (boolean interrupted) {
+
+    }
+}

src/main/java/com/spartronics4915/frc2020/subsystems/Climber.java

@@ -26,22 +26,22 @@ public Climber() {
         mClimberNEO = new CANSparkMax(Constants.Climber.kWinchMotorId, MotorType.kBrushless);
     }
 
-    public static void extend() {
+    public void extend() {
         mClimber775Pro.set(ControlMode.PercentOutput, 0.0);
         mClimberNEO.set(0.0);
     }
 
-    public static void winch() {
+    public void winch() {
         mClimber775Pro.set(ControlMode.PercentOutput, 0.0);
         mClimberNEO.set(0.0);
     }
 
-    public static void reverse() {
+    public void reverse() {
         mClimber775Pro.set(ControlMode.PercentOutput, 0.0);
         mClimberNEO.set(0.0);
     }
 
-    public static void stop() {
+    public void stop() {
         mClimber775Pro.set(ControlMode.PercentOutput, 0.0);
         mClimberNEO.set(0.0);
     }

src/main/java/com/spartronics4915/frc2020/subsystems/PanelRotator.java

@@ -1,23 +1,120 @@
 // I'm bad at naming things. Please come up with a better name...
 package com.spartronics4915.frc2020.subsystems;
 
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.ColorSensorV3;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+import com.spartronics4915.frc2020.Constants;
 import com.spartronics4915.lib.subsystems.SpartronicsSubsystem;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.DigitalInput;
 
 public class PanelRotator extends SpartronicsSubsystem {
+    private final CANSparkMax mSpinMotor;
+    private final CANSparkMax mExtendMotor;
+
+    private final DigitalInput mBeamSensorUp;
+    private final DigitalInput mBeamSensorDown;
+
+    private final ColorSensorV3  mColorSensor;
+
+    //TODO: These are essentially random numbers, with the max value based on the images at
+    // https://www.andymark.com/products/infinite-recharge-control-panel-stickr
+    public int[] mMinimumRed = {200, 0, 0};
+    public int[] mMaximumRed = {255, 30, 30};
+
+    //TODO: These are bad and will work in a way that will make you lose, which will be sad
+    public int[] mMinimumGreen = {0, 200, 0};
+    public int[] mMaximumGreen = {30, 255, 30};
+
+    //TODO: These are bad and will work in a way that will make you lose, which will be sad
+    public int[] mMinimumBlue = {0, 200, 200};
+    public int[] mMaximumBlue = {30, 255, 255};
+
+    //TODO: These are bad and will work in a way that will make you lose, which will be sad
+    public int[] mMinimumYellow = {200, 200, 0};
+    public int[] mMaximumYellow = {255, 255, 30};
+
+    public String sensedColor;
+
+    public int red;
+    public int green;
+    public int blue;
 
     public PanelRotator() {
-
+        mBeamSensorUp = new DigitalInput(Constants.PanelRotator.kBeamSensorUpID);
+        mBeamSensorDown = new DigitalInput(Constants.PanelRotator.kBeamSensorDownID);
+        mSpinMotor = new CANSparkMax(Constants.PanelRotator.kSpinMotorID, MotorType.kBrushless);
+        mExtendMotor = new CANSparkMax(Constants.PanelRotator.kExtendMotorID, MotorType.kBrushless);
+        mColorSensor = new ColorSensorV3(I2C.Port.kOnboard);
     }
 
-    public void rotate(double number) {
+    public void raise() {
+        mExtendMotor.set(Constants.PanelRotator.kExtendMotorSpeed);
+    }
 
+    public void lower() {
+        mExtendMotor.set(-Constants.PanelRotator.kExtendMotorSpeed);
     }
 
-    public String color() {
-        return "red but that's a lie";
+    public String getTargetColor() {
+        String color = DriverStation.getInstance().getGameSpecificMessage();
+        return color;
     }
 
-    public void stop() {
+    public String getActualColor() {
+        //TODO: convert to 0-255 for user convenience.
+        /*red =  mColorSensor.getRed();
+        green =  mColorSensor.getGreen();
+        blue =  mColorSensor.getBlue();
+        sensedColor = "sensor is not working";
+        if(mMinimumRed[0] <= red && red <= mMaximumRed[0]){
+            if(mMinimumRed[1] <= green && green <= mMaximumRed[1]){
+                if(mMinimumRed[2] <= blue && blue <= mMaximumRed[2]){
+                    sensedColor = "Red";
+                }
+            }
+        }
+        if(mMinimumBlue[0] <= red && red <= mMaximumBlue[0]){
+            if(mMinimumBlue[1] <= green && green <= mMaximumBlue[1]){
+                if(mMinimumBlue[2] <= blue && blue <= mMaximumBlue[2]){
+                    sensedColor = "Blue";
+                }
+            }
+        }
+        if(mMinimumYellow[0] <= red && red <= mMaximumYellow[0]){
+            if(mMinimumYellow[1] <= green && green <= mMaximumYellow[1]){
+                if(mMinimumYellow[2] <= blue && blue <= mMaximumYellow[2]){
+                    sensedColor = "Yellow";
+                }
+            }
+        }
+        if(mMinimumGreen[0] <= red && red <= mMaximumGreen[0]){
+            if(mMinimumGreen[1] <= green && green <= mMaximumGreen[1]){
+                if(mMinimumGreen[2] <= blue && blue <= mMaximumGreen[2]){
+                    sensedColor = "Green";
+                }
+            }
+        }
+        return sensedColor;*/
+        return "placeholder string for color sensing";
+    }
 
+    public boolean getBeamSensorDown(){
+        return mBeamSensorDown.get();
+    }
+
+    public boolean getBeamSensorUp(){
+        return mBeamSensorUp.get();
+    }
+
+    public void spin(double speed){
+        mSpinMotor.set(speed);
+    }
+
+    public void stop() {
+        mSpinMotor.set(0);
+        mExtendMotor.set(0);
     }
 }