grimpeur
This commit is contained in:
parent
78d284d0d1
commit
8c7dc5ab87
@ -18,17 +18,46 @@ public class GrimpeurHaut extends Command {
|
|||||||
|
|
||||||
// Called when the command is initially scheduled.
|
// Called when the command is initially scheduled.
|
||||||
@Override
|
@Override
|
||||||
public void initialize() {}
|
public void initialize() {
|
||||||
|
grimpeur.resetencodeurd();
|
||||||
|
grimpeur.resetencodeurg();
|
||||||
|
}
|
||||||
|
|
||||||
// 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.droite()){
|
||||||
|
grimpeur.resetencodeurd();
|
||||||
|
}
|
||||||
|
else if(grimpeur.gauche()){
|
||||||
|
grimpeur.resetencodeurg();
|
||||||
|
}
|
||||||
|
else if(grimpeur.encoderd()>261){
|
||||||
|
grimpeur.droit(0);
|
||||||
|
}
|
||||||
|
else if(grimpeur.getpitch()<-15){
|
||||||
|
grimpeur.droit(0.6);
|
||||||
|
grimpeur.gauche(-0.6);
|
||||||
|
}
|
||||||
|
else if(grimpeur.getpitch()>15){
|
||||||
|
grimpeur.gauche(0.6);
|
||||||
|
grimpeur.droit(-0.6);
|
||||||
|
}
|
||||||
|
else if(grimpeur.encoderg()>261){
|
||||||
|
grimpeur.gauche(0.6);
|
||||||
|
}
|
||||||
|
else{
|
||||||
|
grimpeur.gauche(0);
|
||||||
|
grimpeur.gauche(0);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
// Called once the command ends or is interrupted.
|
||||||
@Override
|
@Override
|
||||||
public void end(boolean interrupted) {}
|
public void end(boolean interrupted) {
|
||||||
|
grimpeur.droit(0);
|
||||||
|
grimpeur.gauche(0);
|
||||||
|
}
|
||||||
|
|
||||||
// Returns true when the command should end.
|
// Returns true when the command should end.
|
||||||
@Override
|
@Override
|
||||||
|
@ -45,7 +45,12 @@ public void resetencodeurd(){
|
|||||||
public void resetencodeurg(){
|
public void resetencodeurg(){
|
||||||
grimpeurg.getEncoder().setPosition(0);
|
grimpeurg.getEncoder().setPosition(0);
|
||||||
}
|
}
|
||||||
|
public double encoderd(){
|
||||||
|
return grimpeurd.getEncoder().getPosition();
|
||||||
|
}
|
||||||
|
public double encoderg(){
|
||||||
|
return grimpeurg.getEncoder().getPosition();
|
||||||
|
}
|
||||||
public AHRS gyroscope = new AHRS();
|
public AHRS gyroscope = new AHRS();
|
||||||
public double getpitch(){
|
public double getpitch(){
|
||||||
return gyroscope.getPitch();
|
return gyroscope.getPitch();
|
||||||
|
Loading…
x
Reference in New Issue
Block a user