Robot not taking input

[sasha831]

Hello,
In the off season our team has been experimenting with Java, and i recently rewrote our Java code to function more efficiently. Unfortunately, With this new code, absolutely nothing works. The Robot does not drive and nothing runs. If i run the old code, then everything works, so it is a software issue. Also, i do not know why but i keep getting a robot drive not running fast enough error with the new code. Here is both the old and new code:

NEW CODE:

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Timer;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTemplate extends SimpleRobot {
    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    
    //Declare drive base, jaguars and compressor
    RobotDrive myBase = new RobotDrive(1, 2);
    Joystick joystick = new Joystick(1);
    Jaguar shootingFront = new Jaguar(3);
    Jaguar shootingBack = new Jaguar(4);
    Jaguar push = new Jaguar(6);
    Jaguar roll = new Jaguar(5);
    Compressor compressor = new Compressor(1,1);
    Solenoid solenoidPistonOut = new Solenoid(1);
    Solenoid solenoidPistonIn = new Solenoid(2);
    DigitalInput topMicro = new DigitalInput(1);
    DigitalInput bottomMicro = new DigitalInput(2);
    
    double shooterMotorFront = 0;
    double shooterMotorBack = 0;
    double pushMotor = 0; 
    double rollMotor = 0;
    double xStick = 0;
    double yStick = 0;
    boolean pistonOut = false;
    boolean pistonIn = false;
    
    public void runMotor() {
        shootingFront.set(shooterMotorFront);
        shootingBack.set(shooterMotorBack);
        push.set(pushMotor);
        roll.set(rollMotor);
        solenoidPistonIn.set(pistonIn);
        solenoidPistonOut.set(pistonOut);
    }

    public void autonomous() {
        int state = 1;
        int c = 0;
        
        switch(state) {
                case 1:
                    shooterMotorFront = 0.8;
                    shooterMotorBack = 0.8;
                    Timer.delay(3);
                    state = 2;
                break;
                case 2:
                    pistonIn = false;
                    pistonOut = true;
                    Timer.delay(1);
                    state = 3;
                break;
                case 3:
                    pistonOut = false;
                    pistonIn = true;
                    Timer.delay(1);
                    state = 4;
                break;
                case 4:
                    if (c == 1) {
                        while(topMicro.get() == true) {
                           rollMotor = 0.4;
                        }
                        state = 2;
                    }
                    else if(c == 2) {
                        while(bottomMicro.get() == true) {
                            rollMotor = 0.4;
                        }
                        state = 2;
                    }
                    else if (c == 3) {
                        state = 5;
                    }
                break;
                case 5:
                    shooterMotorFront = 0;
                    shooterMotorBack = 0;
                    state = 0;
                break;
        }
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {

        myBase.setSafetyEnabled(true);
        int state = 0;
        int c = 0;
        double zStick;
        double map_zStick;
        
        compressor.start();
        
        
        while (isOperatorControl() && isEnabled()) {
            
            if (joystick.getRawButton(6) == true) {
                state = 0;
            }
            
            if (joystick.getRawButton(3)) {
                state = 8;
            }
            
            myBase.arcadeDrive(0.5, 0.5);
            
            switch(state) { 
                case 0:
                    //Get Throttle and map it to a value from 0 to 1
                    zStick = joystick.getAxis(Joystick.AxisType.kZ);
                    map_zStick = (zStick+1)/2;
            
                    //Get x-Axis and map it based on throttle
                    xStick = joystick.getAxis(Joystick.AxisType.kX);
                    xStick = xStick * map_zStick * (-1);
            
                    //Get y-Axis and map it based on throttle
                    yStick = joystick.getAxis(Joystick.AxisType.kY);
                    yStick = yStick * map_zStick * (-1);
                    
                    if (joystick.getRawButton(2) == true) {
                        state = 1;
                    }
                break;
                case 1:
                    int i =0;
                    while(i<3) {
                        rollMotor = 0.4;
                        if (topMicro.get() == true) {
                            i++;
                        }
                    }
                    state = 2;
                break;
                case 2:
                    //Get Throttle and map it to a value from 0 to 1
                    zStick = joystick.getAxis(Joystick.AxisType.kZ);
                    map_zStick = (zStick+1)/2;
            
                    //Get x-Axis and map it based on throttle
                    xStick = joystick.getAxis(Joystick.AxisType.kX);
                    xStick = xStick * map_zStick * (-1);
            
                    //Get y-Axis and map it based on throttle
                    yStick = joystick.getAxis(Joystick.AxisType.kY);
                    yStick = yStick * map_zStick * (-1);
                    
                    myBase.arcadeDrive(yStick, xStick);
                    
                    if (joystick.getRawButton(2) == true) {
                        state = 3;
                    }
                break;
                case 3:
                    shooterMotorFront = 0.8;
                    shooterMotorBack = 0.8;
                    Timer.delay(3);
                    state = 4;
                break;
                case 4:
                    pistonIn = false;
                    pistonOut = true;
                    Timer.delay(1);
                    state = 5;
                break;
                case 5:
                    pistonOut = false;
                    pistonIn = true;
                    Timer.delay(1);
                    state = 6;
                break;
                case 6:
                    if (c == 1) {
                        while(topMicro.get() == true) {
                           rollMotor = 0.4;
                        }
                        state = 4;
                    }
                    else if(c == 2) {
                        while(bottomMicro.get() == true) {
                            rollMotor = 0.4;
                        }
                        state = 4;
                    }
                    else if (c == 3) {
                        state = 7;
                    }
                break;
                case 7:
                    shooterMotorFront = 0;
                    shooterMotorBack = 0;
                    state = 0;
                break;
                case 8:
                    long end = System.currentTimeMillis() + 10000;
                    while(System.currentTimeMillis() < end) {
                        yStick = 0.6;
                    }
                    state = 9;
                break;
                case 9:
                    if (joystick.getRawButton(11) == true) {
                        state = 0;
                    }
                break;
            }
            
            Timer.delay(0.01);
            
            runMotor();
        }
        compressor.stop();
    }
    /**
     * This function is called once each time the robot enters test mode.
     */
    public void test() {
    
    }
}

OLD CODE:

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Timer;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTemplate extends SimpleRobot {
    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    
    //Declare drive base, jaguars and compressor
    RobotDrive myBase = new RobotDrive(1, 2);
    
    Joystick joystick = new Joystick(1);
    
    Jaguar shootingFront = new Jaguar(3);
    Jaguar shootingBack = new Jaguar(4);
    Jaguar push = new Jaguar(6);
    Jaguar roll = new Jaguar(5);
    
    Compressor compressor = new Compressor(1,1);
    Solenoid solenoidPistonOut = new Solenoid(1);
    Solenoid solenoidPistonIn = new Solenoid(2);
    
    double shooterMotorFront;
    double shooterMotorBack;
    double pushMotor; 
    double rollMotor;
    boolean pistonOut = false;
    boolean pistonIn = false;
    
    public void runMotor() {
        shootingFront.set(shooterMotorFront);
        shootingBack.set(shooterMotorBack);
        push.set(pushMotor);
        roll.set(rollMotor);
        solenoidPistonIn.set(pistonIn);
        solenoidPistonOut.set(pistonOut);
    }

    public void autonomous() {
        
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {

        myBase.setSafetyEnabled(true);
        boolean kTriggerButton=false;        
        boolean kTopButton=false;
        boolean button3=false;
        
        compressor.start();
        
        while (isOperatorControl() && isEnabled()) {
            //Get Throttle and map it to a value from 0 to 1
            double zStick = joystick.getAxis(Joystick.AxisType.kZ);
            double map_zStick = (zStick+1)/2;
            
            //Get x-Axis and map it based on throttle
            double xStick = joystick.getAxis(Joystick.AxisType.kX);
            xStick = xStick * map_zStick * (-1);
            
            //Get y-Axis and map it based on throttle
            double yStick = joystick.getAxis(Joystick.AxisType.kY);
            yStick = yStick * map_zStick * (-1);
            
            //Set Robot Drive to mapped Joystick Values
            myBase.arcadeDrive(yStick, xStick);
            
            //Check Trigger and set Shooting Motors accordingly
            if(joystick.getButton(Joystick.ButtonType.kTrigger)==true && kTriggerButton==false) {
                shooterMotorFront = -0.8;
                shooterMotorBack = -0.8;
                kTriggerButton=true;
            }
            else if(joystick.getButton(Joystick.ButtonType.kTrigger)==false && kTriggerButton==true) {
                shooterMotorFront = 0;
                shooterMotorBack = 0;
                kTriggerButton=false;
            }
            
            //Check button 2 and set pushing motor accordingly
            if(joystick.getButton(Joystick.ButtonType.kTop)==true && kTopButton==false) {
                pushMotor = 0.8;
                kTopButton=true;
            }
            else if(joystick.getButton(Joystick.ButtonType.kTop)==false && kTopButton==true) {
                pushMotor = 0;
                kTopButton=false;
            }
            
            //Check Buton 3 an set top wheel accordingly
            if(joystick.getRawButton(3)==true && button3==false) {
                rollMotor = 0.4;
                button3=true;
            }
            else if(joystick.getRawButton(3)==false && button3==true) {
                rollMotor = 0;
                button3=false;
            }
            
            if (joystick.getRawButton(4)) {
                pistonOut = true;
            }
            else if (joystick.getRawButton(5)) {
                pistonIn = true;
            }
            else {
                pistonOut = false;
                pistonIn = false;
            }
            
            Timer.delay(0.01);
            
            runMotor();
        }
        compressor.stop();
    }
    /**
     * This function is called once each time the robot enters test mode.
     */
    public void test() {
    
    }
}

Any help would be greatly appreciated!

Sasha
Team Melbourne
FRC #4529

[Mark McLeod]

It seems like autonomous will never end since the value of c never changes.
You won't see that if you aren't running auto though.

Same problem with c in operatorControl.

You may not have instantiated some variables correctly and the code is aborting.
Are you using NetConsole to check for code errors? That will show you any errors of that type.

[Jon Stratis]

I don't think your Autonomous section is doing what you want. The two while loops:

while(topMicro.get() == true) {
rollMotor = 0.4;
}


while(bottomMicro.get() == true) {
rollMotor = 0.4;
}

These don't actually set the motor speed during autonomous, they only set your local double variables. In fact, I don't see anywhere in the autonomous code where you actually send the values to the motors.

You're probably starting up in Autonomous and seeing nothing happen (because no motors are set to actual values). You are also likely getting stuck in the loops above.

In teleop, I see a similar while loop:

while(i<3) {
rollMotor = 0.4;
if (topMicro.get() == true) {
i++;
}
}

Again, this doesn't actually send the rollMotor value to the motor - you set the local double variable, but are stuck in the loop because i never increments - you don't ever get down to the point where you send values to the controller.

It's very important to watch how you use loops when dealing with this robot code!

Edit: oops... didn't notice that c never changed from 0... I guess you won't hit those loops!

[otherguy]

Re previous comments, I don't see where 'c' is being used to control a loop. So I don't think that's the cause of the robot "doing nothing".
In auto there isn't a loop (which is a problem) except for in state 4, and it has already been identified that these loops will never get executed. Not having a loop wrapping the case statements is a problem since you're never going to go through more than one case in the case statement. the autonomous() method is only called once (upon entering the mode) in the [url="http://team2168.org/javadoc/edu/wpi/first/wpilibj/SimpleRobot.html"]SimpleRobot[url] class.
You're right that 'c' is never modified though, so if you were to get to state 4 in auto, you would never be able to leave it.

I would suggest tryting to re-organize your code a bit:
In autonomous and operator control methods, you should have a while loop wrapping the majority of the code.
The while loop should terminate when you're not in the respective mode (isAutonomous() or isOperatorControl()) and the robot isn't disabled (isDisabled()).

Your case staments are set up to drive a state machine. This way of organizing your code is fine in practice. But as others noted, the code isn't outputting commands to the motors. I would suggest reorganizing your case statements such that they follow the following general outline.

while( you're in the mode and not disabled) {
  switch(state) {
    case X:
      //logic specific to the state (like checking joystick buttons)
      //set the speeds for your motors, note you may want to set the speeds
      //  for ALL motors in each state, or you risk commanding a motor
      //  to a stale value (set by some previous state).
      //Modify your state variable
      //Set a variable (e.g. double dealyTime) for how long to delay
      //  If you don't need to delay, set to zero.
    break;

    case ...:
      //you should have something similar for the remaining cases.
      //each case should
    break;
   
    default:
      //you should ALWAYS define a default case, if state ever got
      //  set to something you didn't have a case for the world will
      //  end. Note you do this in the auto code (case 4, sets state
      //  to 0, but there is not case for state=0!).
      //Since getting here is probably abnormal, you should set safe
      //  default values and maybe change into a safe starting state.
  }

  //outside the switch but inside the while
  // Output to your motors and call the delay
  runMotor();
  Timer.delay(delayTime);
}

Also note, that the way the state machine is set up right now
Ideally you wouldn't have any significant delays (greater than 50ms). It is better to have your states log the time they started, and not change into the next state until the current time exceeds the logged time plus the time you wanted to wait for. This allows the while loop to continue to run (many many times while you are stuck in a sparticular state). If your case statements are set up right this will allow you to continue to respond to operator input while in any particular state.
If you don't set the states up this way then your robot will be unresponsive to joystick commands for periods of time (however long you are delaying for). For example, once you have started to shoot, if someone bumpped into your robot, there is no way for you to correct your alignment until after your shot sequence has completed.


I would say continue to try to implement things the way you are until you get them running. It will be an invaluable learning experience.
If you have time after this to try some more things in java, I would suggest taking a look at the command based robot project. Your state machine design means your already thinking along the right lines to come up with procedures or sequences of actions which your robot needs to perform. The command based robot project gives you a great foundation to build complex tasks upon. And its structure will help you organize your thoughts into code. My team has been using it sucessfully for the past two years.
There's a lot to digest, but it's pretty straight forward. And there is a tool called robot builder which can help generate code for you so that you may get your feet wet.

[sasha831]

Thank You for the help!

I have not been running autonomous, and when i enter teleop it starts in case 0. Since i have no loops running then, why is nothing happening.

@Other Guy, If i change case 0 to default, will that help fix the issue, Also, we are considering using a command based robot, could you recommend any good tutorials?

@Jon Stratis, Thank you for pointing out that we're not sending any values to motors in autonomous! I will fix that and add a while loop

[otherguy]

I'm not sure why nothing moves.
You haven't provided any error messages, so it makes things a little difficult to debug.

One thing I did notice is that you have motor safety enabled. While that might sounds like a good thing to do, you should probably understand what it means. Since the javadocs are pretty much useless in this case, i'll describe it for you.
Wen motor safety is enabled on robot drive. The drive object must be updated faster than every tenth of a second otherwise the MotorSafety class considers your program to be having problems, and it stops outputs to the motors.

This is most certainly not how you want your motors to work if you're going to have code on your robot that delays for seconds at a time. It could be contributing to your "nothing is happening" problem. Since your loop will take more than 0.1 seconds to complete (you have code to execute plus a wait of 0.1s in the operator control method).
Just taking that out may not solve your problem, but it could be one of many things wrong with the code. If it were the only problem I would expect on enabling the robot that the arcadeDrive() call before the while loop would cause the robot to move momentarily at least for a tenth of a second. I would recommend taking setSafetyEnabled out of your code, at least until you know the rest of your code works.

Also, this is a good example of why it's important to test your code as you are developing, it helps you localize problems in the code. Right now you have a completely untested set of code, so you can't be confident that certain areas of it work reliably or as expected. Cut your program down to the simplest it can be (comment large blocks out) until you get something that works, then slowly add things in until it breaks, fix the problems, then continue on to adding more stuff until you've tested it all. Trying to tackle it all at once is (as you have experienced) unmanageable.

[edit]
To answer your question on command based tutorials. I would start with the links I posted in my original response. Those are the materials developed for the 2013 season.
There is also a comprehensive PDF from the 2012 season here. This is what I learned from.
There's a summary with some links to materials which I wrote up for the purposes of teaching my team.
And I saw a bunch of youtube videos, but haven't watched them so can't say if they are good or not.

[Joe Ross]

Mark suggested looking for errors in netconsole (which also is printed in the output window in netbeans. Have you done that?

I suspect you'll find that the code is crashing due to allocating digital input 1 twice. Once in the compressor object and once in the topMicro object. Your old code did not use any digital input objects.