Issue with autonomous mode and some newbie questions

[professorX]

Hi,

Our code for autonomous is not working properly. We programmed the robot to drive straight for 3 seconds and then stop. Instead of driving forward for 3 seconds and then stopping, the robot waits 10 seconds and drives forward for half a second, then waits another 10 seconds and then drives forward for half a second. We believe that it has to be something wrong with feeding the watchdog since we receive the watchdog not fed error on the dashboard.

4 Questions.
1. Could anyone take a look at the code below and tell us what our problem is with the autonomous code?
2. Also, if someone could take a look at our line sensor code to see if it will work or not, that would be great
3. I'm new to netbeans, how do you save as project (not .java file) on a usb drive or on a different location from where it was originally saved?
4. How can I debug the code using System.out.println? It never prints out anything. Are there other ways for debugging the code or knowing what values you get from sensors?

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.AnalogTrigger;
import edu.wpi.first.wpilibj.DigitalInput;
/**
 * 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 Team1660Robot extends SimpleRobot {

    public Team1660Robot (){
        getWatchdog().setExpiration(.5);
       

    }


    /**
     * This function is called once each time the robot enters autonomous mode.
     */

    AnalogTrigger Bumper = new AnalogTrigger (1);
    Joystick rightjoystick = new Joystick(1);
    Joystick leftjoystick = new Joystick(2);
    RobotDrive robotdrive = new RobotDrive(1,2,3,4);

    DigitalInput Mid;
    DigitalInput Right;
    DigitalInput Left;
    
    Timer timer = new Timer();
    double straight = 0;
    double standardRobotSpeed = 0.5;
    double stop = 0;
    double velocity = 3;

    public void Team1660Robot(){
        Mid = new DigitalInput(2);
        Right = new DigitalInput(1);
        Left = new DigitalInput(3);
        getWatchdog().setExpiration(.5);
        getWatchdog().setEnabled(true);
    }

       public void autonomous() {
getWatchdog().setEnabled(true);
getWatchdog().feed();
       // robotdrive.tankDrive(.5,.5);
        //getWatchdog().setEnabled(false);
       // getWatchdog().feed();
       // robotdrive.tankDrive(.5,.5);
        for(int i =0; i < 10; i=+1){
            getWatchdog().feed();
            robotdrive.drive(1,0);
            Timer.delay(3);
            getWatchdog().feed();
        }
        robotdrive.drive(0,0);
        //giveADogABone(3);
       // robotdrive.tankDrive(0,0);
        //bumper();
       // followTheLine();

    }
    public void followTheLine() {
        Timer Clock = new Timer();
        robotdrive.tankDrive(.5, .5);
        Timer.delay(10);
        robotdrive.drive(0, 0);
        
        Clock.start();
        Clock.reset();
        //Clock.stop();

        if(true)
        {
            robotdrive.drive(1, 0);
            Timer.delay(5);
            robotdrive.drive(0, 0);
        }
       
        //boolean End = true;
        /*
        for(int i=0;i<10000;++i) {

            boolean MidValue = Mid.get() ? true: false;
            boolean RightValue = Right.get() ? true: false;
            boolean LeftValue = Left.get() ? true : false;
            double Velocity;
            double direction;
            if (MidValue == true && LeftValue == false && RightValue== false) {
                Velocity = 5;
                direction = 0;
                // Go Straight, tape is in center
            } else if (MidValue == true && LeftValue == true && RightValue == false){
                Velocity = .25;
                direction = -0.5;
               // Go left if tape is on slight Left//
            } else if (MidValue == true && LeftValue == false && RightValue == true){
                Velocity = .25;
                direction = 0.5;
                //Go right if tape is on slight Right//
            } else if (MidValue == false && LeftValue == true && RightValue == false){
                Velocity = .25;
                direction = -0.7;
                //Go Left if tape is on extreme Left//
            } else if (MidValue == false && RightValue == true && LeftValue == false){
                Velocity = .25;
                direction = 0.7;
                //Go Right if tape is on extreme Right//
            } else {
                Velocity = .25;
                direction = -1;


            }

           robotdrive.drive(-Velocity,-direction);

           if(Clock.get()>15)
               break;
           /* if (UseWhite) {
                A = RedValue *2 + WhiteValue *4 + BlackValue;
            } else if (UseBlack) {
                A = RedValue *2 + BlackValue *3 + WhiteValue;
            }
               else if (UseRed){
                   A = RedValue * 2 + BlackValue + WhiteValue;

            A=2;

            Velocity =2* powerProfile[timeInFull];
           turn = 0;

           switch (A) {
               case 2:
                   turn = 0;
               break;
               case 6:if(time > ClockStop) {
                   End = true;
                   Velocity = 0.0;
               }

               break;
               default: if  (A != B) {
                System.out.println("Time: " + time + " Sensor,Case: " + A + " Velocity: " + Velocity + " turn: " + turn + " End: " + End);
                   }

               break;
               Velocity = robotdrive.getVelocity;
               if (A != 0) { A = B;}
        }
        }
            *
            */


        //}

        //robotdrive.drive(stop,straight);
    }


    public void giveADogABone(double timeToWait) {
         double WatchdogDelay = 4.9;
         while (timeToWait > 0){
             getWatchdog().feed();
             if (timeToWait < WatchdogDelay){
                 Timer.delay (timeToWait);
                }
             else {
                 Timer.delay (WatchdogDelay);
             }
             timeToWait = timeToWait - WatchdogDelay;
         }
     }



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


        while(isEnabled() && isOperatorControl())
        {
            getWatchdog().feed();
            robotdrive.tankDrive(leftjoystick, rightjoystick);

            if( rightjoystick.getRawButton(8) == true) {
                getVelocity();
            }
        }
    }

    public double distance_to_time (double distance){
        double time = distance / velocity;
        return (time);
    }

    public void getVelocity (){


        robotdrive.drive(standardRobotSpeed,straight);
        //5 is the seconds to test
        Timer.delay(5);
        robotdrive.drive(stop,straight);
    }
    public void bumper(){
        int x = 1;

        while (x == 1){
            if(Bumper.getTriggerState() == false){
                robotdrive.drive(standardRobotSpeed,straight);
            }
            else {
                x = 2;
                robotdrive.drive(stop,straight);
            }
        }
    }
        }

[Robby Unruh]

Your problem lies here:

for(int i =0; i < 10; i=+1){
    getWatchdog().feed();
    robotdrive.drive(1,0);
    Timer.delay(3);
    getWatchdog().feed();
}

You're using a timer delay in a loop. That means that everytime the loop is called (which is two, for whatever reason) you will have a 3 second delay. Plus, since you're wanting to drive for a set amount of time in autonomous, why not use the nifty Timer class?

Timer timer = new Timer(); // you should put this in your constructor.

public void autonomous() {
    while(isAutonomous()) {
        timer.start();
        while(timer.get() < 3) { // while the timer is < 3, drive forward.
            robotDrive.drive(1,0);
            timer.stop(); // stop the timer after 3 seconds
        }
        robotDrive.drive(0,0); // stop the robot from driving any further.
    }
}

[professorX]

Originally Posted by Robby Unruh

Your problem lies here:

for(int i =0; i < 10; i=+1){
    getWatchdog().feed();
    robotdrive.drive(1,0);
    Timer.delay(3);
    getWatchdog().feed();
}

You're using a timer delay in a loop. That means that everytime the loop is called (which is two, for whatever reason) you will have a 3 second delay. Plus, since you're wanting to drive for a set amount of time in autonomous, why not use the nifty Timer class?

Timer timer = new Timer(); // you should put this in your constructor.

public void autonomous() {
    while(isAutonomous()) {
        timer.start();
        while(timer.get() < 3) { // while the timer is < 3, drive forward.
            robotDrive.drive(1,0);
            timer.stop(); // stop the timer after 3 seconds
        }
        robotDrive.drive(0,0); // stop the robot from driving any further.
    }
}

I thought the timer delay pauses the code. Since the motors weren't told to stop before the timer delay so it should continue straight for 3 seconds. And it should do it 10 times according the for loop.
I think the issue is that the watchdog is not being fed during the pause so the robot shuts down. Could this be the issue and how could it be fixed?

[mwtidd]

email me at mwtidd@gmail.com

I converted your code to my ADK which does most the work for you.

Lets you focus on the important stuff.

[Robby Unruh]

Originally Posted by professorX

I thought the timer delay pauses the code. Since the motors weren't told to stop before the timer delay so it should continue straight for 3 seconds. And it should do it 10 times according the for loop.
I think the issue is that the watchdog is not being fed during the pause so the robot shuts down. Could this be the issue and how could it be fixed?

In autonomous, watchdog must be set to false.

Watchdog.getInstance().setEnabled(false);

Might be your problem.

[professorX]

Thanks for the help. I will let you know how it goes when I test out the code on Monday.

Can someone answer questions 3 & 4 in the first post?
3. How do you save as project (not .java file) on a usb drive or on a different location from where it was originally saved?
4. How can I debug the code using System.out.println? It never prints out anything. Are there other ways for debugging the code or knowing what values you get from sensors?

[lusterlink]

Originally Posted by professorX

Thanks for the help. I will let you know how it goes when I test out the code on Monday.

Can someone answer questions 3 & 4 in the first post?
3. How do you save as project (not .java file) on a usb drive or on a different location from where it was originally saved?
4. How can I debug the code using System.out.println? It never prints out anything. Are there other ways for debugging the code or knowing what values you get from sensors?

I believe one way to achieve saving a project in an alternative location is to simply go to the project directory. Copy the project in question and paste it into the usb drive. There might be more elegant methods built into Netbeans, but I haven't found them.

The System.out.println function output should show right up in the Netconsole. Are you getting any output in the Netconsole (e.g. the cRIO spitting out all its boot up stuff)?

[drakesword]

Originally Posted by professorX

Thanks for the help. I will let you know how it goes when I test out the code on Monday.

Can someone answer questions 3 & 4 in the first post?
3. How do you save as project (not .java file) on a usb drive or on a different location from where it was originally saved?
4. How can I debug the code using System.out.println? It never prints out anything. Are there other ways for debugging the code or knowing what values you get from sensors?

3.) As stated above copy the project folder.

4.) println will print to the netconsole (eg the output section of netbeans while it is running). If you don't see output you may need to check your subnet mask. If it is not 255.0.0.0 then you will not see output from the robot.

I also agree that it appears that you are tripping the watchdog. You set the watchdog to get hungry every 0.5 seconds but only feed it every 3.

[Robby Unruh]

4) I use to stop my builds before switching over to the driver station. I actually only (very) recently discovered that I can keep it running to debug my code!

So to answer your question, let's say I have a teleop function like so:

public void operatorControl() {
    while(isOperatorControl()) {
        System.out.println("This code will show up a million times when I enable Teleop from the driver station!");
    }
}

I was shocked that it actually worked. haha