Compare commits
2 Commits
d1f4e11cfb
...
d04817a197
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
d04817a197 | ||
|
|
a45e81349a |
@@ -24,9 +24,12 @@ public class DescendreBalyeuse extends Command {
|
|||||||
// Called every time the scheduler runs while the command is scheduled.
|
// Called every time the scheduler runs while the command is scheduled.
|
||||||
@Override
|
@Override
|
||||||
public void execute() {
|
public void execute() {
|
||||||
if(balayeuse.Distance() < balayeuse.EncodeurBalayeuse()){
|
if(Math.abs(balayeuse.Distance()) < balayeuse.EncodeurBalayeuse()){
|
||||||
balayeuse.Pivoter(-0.2);
|
balayeuse.Pivoter(-0.2);
|
||||||
}
|
}
|
||||||
|
else{
|
||||||
|
balayeuse.Pivoter(0);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
// Called once the command ends or is interrupted.
|
||||||
|
|||||||
@@ -24,7 +24,7 @@ public class DescendreGrimpeur extends Command {
|
|||||||
@Override
|
@Override
|
||||||
public void execute() {
|
public void execute() {
|
||||||
if(!grimpeur.Limit()){
|
if(!grimpeur.Limit()){
|
||||||
grimpeur.Grimper(-0.5);
|
grimpeur.Grimper(-0.2);
|
||||||
}
|
}
|
||||||
else{
|
else{
|
||||||
grimpeur.Reset();
|
grimpeur.Reset();
|
||||||
|
|||||||
@@ -23,8 +23,13 @@ public class MonterGrimpeur extends Command {
|
|||||||
// Called every time the scheduler runs while the command is scheduled.
|
// Called every time the scheduler runs while the command is scheduled.
|
||||||
@Override
|
@Override
|
||||||
public void execute() {
|
public void execute() {
|
||||||
if(grimpeur.Position() < grimpeur.PositionFinal()){
|
|
||||||
grimpeur.Grimper(0.5);
|
if(Math.abs(grimpeur.Position()) < grimpeur.PositionFinal()){
|
||||||
|
grimpeur.Grimper(0.2);
|
||||||
|
System.out.println("monte");
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
grimpeur.Grimper(0);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -39,7 +39,7 @@ public class Balayeuse extends SubsystemBase {
|
|||||||
Pivot.getEncoder().setPosition(0);
|
Pivot.getEncoder().setPosition(0);
|
||||||
}
|
}
|
||||||
public boolean GetLimiSwtich(){
|
public boolean GetLimiSwtich(){
|
||||||
return limit.get();
|
return !limit.get();
|
||||||
}
|
}
|
||||||
public double Amp(){
|
public double Amp(){
|
||||||
return Balaye2.getOutputCurrent();
|
return Balaye2.getOutputCurrent();
|
||||||
|
|||||||
@@ -17,7 +17,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|||||||
public class Grimpeur extends SubsystemBase {
|
public class Grimpeur extends SubsystemBase {
|
||||||
ShuffleboardTab teb = Shuffleboard.getTab("teb");
|
ShuffleboardTab teb = Shuffleboard.getTab("teb");
|
||||||
SparkMax grimpeur1 = new SparkMax(3,MotorType.kBrushless);
|
SparkMax grimpeur1 = new SparkMax(3,MotorType.kBrushless);
|
||||||
SparkMax grimpeur2 = new SparkMax(4,MotorType.kBrushless);
|
SparkMax grimpeur2 = new SparkMax(12,MotorType.kBrushless);
|
||||||
SparkMaxConfig slaveConfig = new SparkMaxConfig();
|
SparkMaxConfig slaveConfig = new SparkMaxConfig();
|
||||||
DigitalInput limit = new DigitalInput(0);
|
DigitalInput limit = new DigitalInput(0);
|
||||||
private GenericEntry EncodeurGrimpeur =
|
private GenericEntry EncodeurGrimpeur =
|
||||||
|
|||||||
Reference in New Issue
Block a user