I am trying to get an autonomous that simply places a cube in the high node and then backs up till the gyro sensor reads greater than 14 degrees(cause we are on the charge station) then stop driving and deploy our robots “brakes”(squish wheels deployed by pneumatics). My code works fine up till the point that it is supposed to back up, where it just doesn’t move. Another auto mode I have is supposed to place the piece, back up, turn around, and then grab another piece. I have the same problem with this. It place the piece, then does nothing before going through the arm and claw actions required to grab another game piece without moving the wheels at all. We are running mechanum drive with NEOs. I have attached the code below. Any help at all is appreciated! Also this is my first post here, so not sure how to make the code look good, sorry.
if(m_timer.get() < 0.4){
UpperArm.set(Value.kForward);
BottomArm.set(Value.kForward);
//Set arm to top position
}
if(m_timer.get() > 0.6 && m_timer.get() < 3){
WristAngler.set(wristController.calculate(encoder.get(), 870));
//set wrist to top position
}
if(m_timer.get() > 3 && m_timer.get() < 3.5){
WristAngler.set(0.0);
m_Drive.driveCartesian(0, 0.4 / 3, 0);
//stop wrist and drive forward
}
if(m_timer.get() > 3.5 && m_timer.get() < 3.6){
RightClaw.set(Value.kForward);
clawClosed = false;
//open claw to release game piece
}
if(m_timer.get() > 3.6 && m_timer.get() < 4.6){
m_Drive.driveCartesian(0, -0.3 / 3, 0);
//back up
}
if(m_timer.get() > 3.6 && m_timer.get() < 4.6){
WristAngler.set(wristController.calculate(encoder.get(), bottomWrist));}
//set wrist to bottom position
if(m_timer.get() > 4.4 && m_timer.get() < 5.4){
UpperArm.set(Value.kReverse);
BottomArm.set(Value.kReverse);
WristAngler.set(wristController.calculate(encoder.get(), bottomWrist));
//set wrist and arm to bottom position
}
if(m_timer.get() > 5.4 && (gyro.getYComplementaryAngle() < 14 || gyro.getYComplementaryAngle() > -14)){
m_Drive.driveCartesian(0, -1 / 3, 0);
//drive backwards(this is where the program stops working)
}
if(gyro.getYComplementaryAngle() > 14 || gyro.getYComplementaryAngle() < -14){
Brakes.set(Value.kForward);
WristAngler.set(0.0);
m_Drive.driveCartesian(0, 0, 0);
//stop driving and deploy brakes
}
I suspect gyro.getYComplementaryAngle() isn’t returning what you’re expecting so it’s falling into that last if. Do you log that value anywhere to confirm it?
Also, num < x || num > -x is inclusive of all possible numbers, so that’s likely not what you’re intending
Yes, I have confirmed that getYComplementaryAngle() returns the value I need. The first if statement(that has timer and gyro angle) is supposed to stay on until the gyro reads greater than 14 or less than -14. The second one is supposed to turn on when the gyro reds greater than 14 or less than -14. I think it should be doing that? Although please correct me if I’m mixing something up
Your first if that checks gyro angle looks like you want to find a value BETWEEN -14 and 14, right? So step through the logic bit by bit using, say, 20.
Spoilers!
It should be false as its not between, but you’ll find that it actually passes the > -14 condition and since you have it OR’d, it will evaluate to true.
What you like want for the 2 conditions is > -14 && < 14 for the first, and < -14 || > 14 for the second.
Edit: I just noticed why you’re not driving reverse. The classic blunder known as integer division! -1 / 3 will not evaluate how youd expect (but -1.0 / 3.0 should)
I understand the gyro and angle bit now! Thanks for that help! And yeah, I found out about integer division too from another source too. Would have been a great thing to know 4 hours ago when I had the robot sitting in front of me! Wouldn’t it be nice if stuff just worked! Thanks for all your help!!
I’m fascinated by other team’s code. There are so many ways to do very similar things. I would suggestion your team investigate a case/switch state machine next season.
Nice catch by Fletch137 on the integer division. In most of the code I write for work, we are required explicitly show the type. For example, no 0 by itself, only double(0.0) or int(0) as needed.
Definitely try command-based. In the past, you had to learn a bunch of object-oriented mumbo-jumbo to use it correctly, but these days it’s basically just a set of tools for defining higher-order robot functions that sequence your actions for you. There’s barely any code footprint - the days of having to define a whole library of classes to use the framework are long gone.