-
Notifications
You must be signed in to change notification settings - Fork 15
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge branch 'master' of https://github.com/Spartronics4915/2020-Infi…
- Loading branch information
Showing
4 changed files
with
161 additions
and
9 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
37 changes: 37 additions & 0 deletions
37
src/main/java/com/spartronics4915/frc2020/commands/SpinToColorCommand.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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) { | ||
|
||
} | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
107 changes: 102 additions & 5 deletions
107
src/main/java/com/spartronics4915/frc2020/subsystems/PanelRotator.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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); | ||
} | ||
} |