Compare commits
2 Commits
56b28100df
...
7b734cc046
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
7b734cc046 | ||
|
|
cdca0230a4 |
@@ -56,7 +56,7 @@ public class BougerGaucheAuto extends Command {
|
|||||||
}
|
}
|
||||||
else{
|
else{
|
||||||
if(timer.get() < 0.75){
|
if(timer.get() < 0.75){
|
||||||
drivetrain.setControl(drive.withVelocityY(-1.5));
|
drivetrain.setControl(drive.withVelocityY(1.5));
|
||||||
}
|
}
|
||||||
else{
|
else{
|
||||||
drivetrain.setControl(drive.withVelocityY(0));
|
drivetrain.setControl(drive.withVelocityY(0));
|
||||||
|
|||||||
@@ -65,7 +65,7 @@ public class GrimperMur extends Command {
|
|||||||
angle = angle + 360;
|
angle = angle + 360;
|
||||||
}
|
}
|
||||||
if(alliance.get() == Alliance.Blue){
|
if(alliance.get() == Alliance.Blue){
|
||||||
y = 5;
|
y = 5.4;
|
||||||
x = 1.11;
|
x = 1.11;
|
||||||
angle = 0;
|
angle = 0;
|
||||||
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 350 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 10){
|
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 350 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 10){
|
||||||
@@ -81,7 +81,7 @@ public class GrimperMur extends Command {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
else{
|
else{
|
||||||
x = 11.45966;
|
x = 15.6;
|
||||||
y = 6.959326;
|
y = 6.959326;
|
||||||
angle = 180;
|
angle = 180;
|
||||||
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170 && drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190){
|
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170 && drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190){
|
||||||
|
|||||||
@@ -70,10 +70,10 @@ public class GrimperReservoir extends Command {
|
|||||||
pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
|
pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
|
||||||
System.out.println(pigeonAngle);
|
System.out.println(pigeonAngle);
|
||||||
y = 5.4;
|
y = 5.4;
|
||||||
x = 15.4;
|
x = 15.6;
|
||||||
angle = 180;
|
angle = 180;
|
||||||
if(pigeonAngle< 190 && pigeonAngle> 170){
|
if(pigeonAngle< 190 && pigeonAngle> 170){
|
||||||
drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5.5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5.5),-2,2)));
|
drivetrain.setControl(drive.withVelocityY(MathUtil.clamp((-(y-boty)*5.5), -2, 2)).withVelocityX(MathUtil.clamp((-(x-botx)*5.5),-2,2)));
|
||||||
}
|
}
|
||||||
else{
|
else{
|
||||||
if(pigeonAngle>180){
|
if(pigeonAngle>180){
|
||||||
|
|||||||
Reference in New Issue
Block a user