Robot driving off.

[Bluedog24]

Why would our robot try to run off? After I uploaded the code and enabled it on the driver station, without touching the joystick, the wheels started running at full speed and wouldn't stop.

Here's the code and I was using a Logitech Dual Action controller.

package edu.wpi.first.wpilibj.templates;



import edu.wpi.first.wpilibj.*; //Imports everything


public class RobotTemplate extends SimpleRobot {
RobotDrive drive = new RobotDrive(1,2,3,4); //Signal wires from jaguars to digital sidecar
Joystick stick = new Joystick(1); //Logitech Dual Action controller
Joystick arm = new Joystick(2); //Logitech Attack 3 controller

public RobotTemplate(){
    getWatchdog().setExpiration(0.5);
    
    compressor.start();
    
}

    public void autonomous() { //Autonomous
        
    }


    public void operatorControl() { 
   while (true && isOperatorControl() && isEnabled()) { 
       getWatchdog().feed();
       drive.mecanumDrive_Polar(stick.getDirectionDegrees(), stick.getMagnitude(),stick.getRawAxis(4));
       Timer.delay(0.005); 


        }
    }
}

Any ideas?

[Robby Unruh]

More than likely it's a problem in mecanumDrive_polar itself. I have heard some nasty things from other teams about it.

What I had to eventually do was program my own drive train (which works awful, but that's what the remainder of the season is for!) until we got a mecanum drive that suits our liking.

But before you go off and make your own, try getting rid of the Timer.delay(0.005); line. I don't know why you'd want to delay your operatorControl, as it would just make your robot move in little spurts.

[omalleyj]

drive.mecanumDrive_Polar(stick.getDirectionDegrees(), stick.getMagnitude(),stick.getRawAxis(4));

What kind of joystick are you using? Which axis is 4 on it? How did you determine that? I can't see either of the other two parameters being the issue.

[derekwhite]

Bluedog24,

You can print out the values of your joystick - perhaps they aren't zero as you expect.

Robby,

It's a good idea not run your control loop flat out - you only get new joystick values every 50ms, so delaying 5ms isn't going to hurt anything. This will allow camera code, dashboard code, etc to get a chance to run.

In fact, there's a new method DriverStation.waitForData(long ms) that would be handy to use.

[Robby Unruh]

Originally Posted by derekwhite

In fact, there's a new method DriverStation.waitForData(long ms) that would be handy to use.

First I've heard of that. Thanks for clearing that up.

Originally Posted by omalleyj

drive.mecanumDrive_Polar(stick.getDirectionDegrees(), stick.getMagnitude(),stick.getRawAxis(4));

What kind of joystick are you using? Which axis is 4 on it? How did you determine that? I can't see either of the other two parameters being the issue.

If I'm not mistaken, it's a Dual Action controller. Very similar to PS2/3 controllers.

Axis 4 is the right stick's Y axis.

[omalleyj]

Originally Posted by Robby Unruh

More than likely it's a problem in mecanumDrive_polar itself. I have heard some nasty things from other teams about it.

The sin/cos problem was fixed in the last update

What I had to eventually do was program my own drive train (which works awful, but that's what the remainder of the season is for!) until we got a mecanum drive that suits our liking.

We have used both the wpilibj and our own versions based on the whitepaper available here and both work great. You might want to revisit the library version now that its fixed.

But before you go off and make your own, try getting rid of the Timer.delay(0.005); line. I don't know why you'd want to delay your operatorControl, as it would just make your robot move in little spurts.

Delays (sleeps) are used to allow for other tasks to run. The throughput from the driverstation is limited to the packet transfer rate and running DS dependent code (like joysticks) faster than that is a waste of CPU cycles.

And in the digital world everything runs in little spurts, only real life is continuous.
The trick is to keep the spurts below the threshold where you notice them, like TV or movie images.

[omalleyj]

Originally Posted by derekwhite

Bluedog24,
<snip>
In fact, there's a new method DriverStation.waitForData(long ms) that would be handy to use.

I missed that too, and I don't see it in the version of javadocs I have with me (from the beginning of the season). I've used isNewControlData(), how does waitForData behave? (just tell me to RTF(new)M when I get home if it is now documented )

[Robototes2412]

here is my motor driving code, complete with mecanum

package com.robototes.abomasnow;

import edu.wpi.first.wpilibj.*;

public class Driver {
    private Jaguar  FLjaguar;
    private Jaguar  FRjaguar;
    private Jaguar  RLjaguar;
    private Jaguar  RRjaguar;
    private boolean mecanum;
    private boolean wheel4;

    /*
     * Usage: Driver robot = new Driver(1,2);
     */
    public Driver(int L, int R) {
        FLjaguar = new Jaguar(L);
        FRjaguar = new Jaguar(R);
        wheel4   = false;
        mecanum  = false;
    }

    /*
     * Usage: Driver robot = new Driver(1,2,3,4);
     */
    public Driver(int FL, int FR, int RL, int RR) {
        wheel4   = true;
        FLjaguar = new Jaguar(FL);
        FRjaguar = new Jaguar(FR);
        RLjaguar = new Jaguar(RL);
        RRjaguar = new Jaguar(RR);
        mecanum  = false;
    }

    /*
     * Usage: Driver robot = new Driver(1,2,3,4,true);
     */
    public Driver(int FL, int FR, int RL, int RR, boolean mecanum) {
        wheel4   = true;
        FLjaguar = new Jaguar(FL);
        FRjaguar = new Jaguar(FR);
        RLjaguar = new Jaguar(RL);
        RRjaguar = new Jaguar(RR);
        this.mecanum = true;
    }

    /*
     * This function takes in one input, speed.  If you dont have the mecanum
     * flag defined, it will skip to just setting the speed.  Otherwise, it will
     * set the speeds of the first two Jaguars.  If you are using four wheels
     * in the constructor, it will also set those speeds accordingly.
     */
    public void drive(double speed) {
        if (!mecanum) {
            FLjaguar.set(speed);
            FRjaguar.set(-speed);

            if (wheel4) {
                RLjaguar.set(speed);
                RRjaguar.set(-speed);
            }
        } else {
            this.holonomicDrive(speed, 0, 0);
        }
    }

    /*
     * This takes two inputs, left speed and right speed
     * it sets the first two jaguars
     * then it checks if you called Driver with 4 motor ports
     * if that is true, it sets the values for the back two motors
     */
    public void tankDrive(double left, double right) {
        if (!mecanum) {
            FLjaguar.set(left);
            FRjaguar.set(-right);

            if (wheel4) {
                RLjaguar.set(left);
                RRjaguar.set(-right);
            }
        }
    }

    /*
     * TankDrive, xbox 360 input version
     */
    public void tankDrive(Xbox foo) {
        this.tankDrive(-foo.getLeftStickAxes()[1], -foo.getRightStickAxes()[1]);
    }

    /*
     * Makes sure we dont send a n<1 value to the jaguars, they dont like that
     */
    double limit(double num) {
        if (num > 1.0) {
            return 1.0;
        }

        if (num < -1.0) {
            return -1.0;
        }

        return num;
    }

    double maxOf4(double one, double two, double three, double four) {
        return Math.max(Math.max(Math.abs(one), Math.abs(two)), Math.max(Math.abs(three), Math.abs(four)));
    }

    /*
     * This brilliant hack was make by our team's all around awesome mentor, Dimitri
     * Dont touch it
     *
     * Takes one input, input.  if the input is positive, it squares it.  If its
     * negative, it squares it while preserving the sign.  It returns the input.
     *
     * It allows greater control of speed while preserving the ability to go at full
     * speed
     */
    public double fiddleWithSpeed(double input) {
        if (input > 0) {
            input = input * input;
        } else {
            input = -input * input;
        }

        return input;
    }

    public void holonomicDrive(double power, double slide, double spin) {
        /*
         * This is a correct mecanum wheel drive function
         */
        double fl, fr, rl, rr;

        fl = power + slide + spin;
        fr = power - slide - spin;
        rl = power - slide + spin;
        rr = power + slide - spin;

        double max = this.maxOf4(fl, fr, rl, rr);

        if (max>1) {
            fl = fl/max;
            fr = fr/max;
            rl = rl/max;
            rr = rr/max;
        }

        fl = limit(/*fiddleWithSpeed*/(fl)); //don't need fiddleWithSpeed for victors, you will for jaguars
        fr = limit(/*fiddleWithSpeed*/(fr));
        rl = limit(/*fiddleWithSpeed*/(rl));
        rr = limit(/*fiddleWithSpeed*/(rr));

        if(power != 0 || slide !=0 || spin != 0) {
            System.out.println("#############################################");
            System.out.println("power: " + power + "  spin: " + spin + "  slide: " + slide);  //debugOut
            System.out.println("FLSpeed: " + fl + "  FRSpeed: " + fr);
            System.out.println("RLSpeed: " + rl + "  RRSpeed: " + rr);
        }

        this.set(fl, fr, rl, rr);
    }

    public void arcadeDrive(double power, double turn) {
        double left,right;

        left = power - turn;
        right = power + turn;

        double max = Math.max(left, right);

        if (max > 1) {
            left = left/max;
            right = right/max;
        }

        left = limit(fiddleWithSpeed(left));
        right = limit(fiddleWithSpeed(right));

        this.set(left, right);
    }

    public void arcadeDrive(JessPad Fool) {
        this.arcadeDrive(-Fool.getLeftStickAxes()[1], Fool.getLeftStickAxes()[0]);
    }

    public void stop() {
        this.drive(0);
    }

    public void set(double left, double right) {
        this.tankDrive(left, right);
    }

    public void set(double FL, double FR, double RL, double RR) {
        this.FLjaguar.set((limit(FL)));
        this.FRjaguar.set((limit(-FR)));
        this.RLjaguar.set(((limit(RL))));
        this.RRjaguar.set((limit(-RR)));
    }

    public double getAVGSpeed() {
    /*
     * If there are 4 wheels defined, return the values of all the jaguar values averaged, otherwise,
     * return the speeds of the two wheels averaged.
     *
     * I know its ternary abuse, but it works (i tested it in javascript)
     */
        return wheel4 //condition to test
                ? (this.FLjaguar.get() + this.FRjaguar.get() + //if its true
                    this.RLjaguar.get() + this.RRjaguar.get())/ 4 //cont.
                : (this.FLjaguar.get() + this.FRjaguar.get()) / 2; //if its false
    }

    public void goForward() {
        this.holonomicDrive(0.5, 0, 0);
    }

    public void goBackward() {
        this.holonomicDrive(-0.5, 0, 0);
    }

    public void turnRight() {
        this.holonomicDrive(0, 0, 0.5);
    }

    public void turnLeft() {
        this.holonomicDrive(0, 0, -0.5);
    }
}

[Bluedog24]

I got it fixed, switched around getDirectionDegrees and getMagnitude.

Thanks for the help.

[derekwhite]

The both get the job done, but there are differences:

• isNewControlData() needs to be polled to determine when new data is ready
• isNewControlData() always clears the internal state "newControlData", so you can't have two threads waiting for new data. Note that the IterativeRobot class calls isNewControlData(), so IterativeRobots can't really call it anywhere else.
• waitForData() uses the standard Java wait/notify routines, and blocks the caller until data is ready (or the time expires).