Code Inefficient?

[oddboy]

Hello,

We are currently at the FIRST Robotics Regional @ Richmond/VCU. We have been getting massive lag during tele op. However, this doesn't seem to be happening during autonomous. What we were told is that our code is "bloated/inefficient/RAM hog,etc". Can some one double check this code?

Here is our code(s):

RobotTemplate:

a/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.DriverStationLCD.Line;


/**a
 * 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 {

   private static Joystick dualShock = new Joystick(1);
   protected static Joystick jordansFolly = new Joystick(2);
   protected static Jaguar jag1 = new Jaguar(1);
   protected static Jaguar jag2 = new Jaguar(2);
   protected static Jaguar shooterJag = new Jaguar(3);
   protected static Victor rampMotor1 = new Victor(4);
   protected static Victor rampMotor2 = new Victor(5);
   protected static double shooterControl;
   protected static RobotDrive drive = new RobotDrive(jag1, jag2);
   // 1 for near, 2 for middle, 3 for far
   // need some way of setting
    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
      // Will hopefully be able to hit all balls if positioned in a line

     getWatchdog().setExpiration(7);
     getWatchdog().feed();
     shooterJag.set(1.0);
     jag1.set(-1.0);
     jag2.set(1.0);
     Timer.delay(1.00);
     jag1.set(0.0);
     jag2.set(0.0);
     shooterJag.set(0.0);
     getWatchdog().setExpiration(20);
     // needs code to turn pi/2 left, move 3 feet, turn back, and repeat above
     // then do again
    }
    

   public void testOutput(String s, Line l) {
        DriverStationLCD lcd = DriverStationLCD.getInstance();
        lcd.println(l, 1, s);
        lcd.updateLCD();
    }
    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        getWatchdog().setEnabled(true);
        //shotDelay1.reset();
        while(true && isOperatorControl() && isEnabled())
          {
            getWatchdog().feed();
          //  Solenoid sol1 = new Solenoid(1);
          //  sol1.set(false);
            //Pneumatics.operate(dualShock, shotDelay1);
            DualShock.drive(dualShock, jag1, jag2, drive);
            shooterControl = jordansFolly.getZ()/-2.0 + 0.5;
            shooterJag.set(shooterControl);



            //THIS WORKS - ml. 2/22

            //raises or lowers ramp while joystick trigger is held down
         //Pressed returns false for some reason
            if (jordansFolly.getTrigger())
            {
                rampMotor1.set(1.0);
                rampMotor2.set(1.0);
            }
            else if (jordansFolly.getRawButton(2))
            {
                rampMotor1.set(-1.0);
                rampMotor2.set(-1.0);
            }
            else
            {
                rampMotor1.set(0.0);
                rampMotor2.set(0.0);
            }

          }
    
    }

}

Code for Logitech DualShock controller:

        /*/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.templates;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.DriverStationLCD.Line;


/**
 *
 * @author Robotics
 */
public class DualShock {

   protected static double left;
   protected static double right;

      public static void testOutput(String s, Line l) {
        DriverStationLCD lcd = DriverStationLCD.getInstance();
        lcd.println(l, 1, s);
        lcd.updateLCD();
    }

    public static void drive(Joystick dual, Jaguar jag1,
            Jaguar jag, RobotDrive drive)

    {


    left = dual.getRawAxis(4); // Y-axis on left stick
    right = dual.getY();
    // left is jag 3, right is jag 2
    drive.setLeftRightMotorSpeeds(left, right);
    Timer.delay(0.005);
    //pause for duration between update cycles of jaguars
    //may not be needed
    }

}

Here is the ZIP file for all the code: http://qwh9pg.blu.livefilestore.com/...e.zip?download

[Robototes2412]

Try making your own driver class as such:

package com.shadowh511.tomo.camera.moving;

import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.DriverStationLCD;
import com.sun.squawk.util.MathUtils;

/**
 *
 * @author sam
 */
public class Driver {
    private Victor FLvictor;
    private Victor FRvictor;
    private Victor RLvictor;
    private Victor RRvictor;
    
    private boolean wheel4;
    private boolean mecanum;

    /*
     * Usage: Driver robot = new Driver(1,2);
     */
    public Driver(int L, int R) {
        FLvictor = new Victor(L);
        FRvictor = new Victor(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;
        FLvictor = new Victor(FL);
        FRvictor = new Victor(FR);
        RLvictor = new Victor(RL);
        RRvictor = new Victor(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;
        FLvictor = new Victor(FL);
        FRvictor = new Victor(FR);
        RLvictor = new Victor(RL);
        RRvictor = new Victor(RR);
        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 Victors.  If you are using four wheels
     * in the constructor, it will also set those speeds accordingly.
     */
    public void drive(double speed) {
        if(!mecanum){
            FLvictor.set(speed);
            FRvictor.set(-speed);

            if(wheel4) {
                RLvictor.set(speed);
                RRvictor.set(-speed);
            }
        } else {
            this.mecanumDrive(speed, 0);
        }
    }

    /*
     * This functon takes in two inputs, speed and curve.  It then creates
     * temporary speed variables.  It borrows WPI's curve calculation algorithm
     * to creatively figure out the individual motor values.  It then calls
     * tankDrive to set the motor speeds.
     */
    public void drive(double speed, double curve) {
        double leftSpeed, rightSpeed;

        if (curve < 0) { //borrowed from edu.wpi.first.wpilibj.RobotDrive, calculates the curve
            double value = MathUtils.log(-curve);
            double ratio = (value - 0) / (value + 0);
            if (ratio == 0) {
                ratio = .0000000001;
            }
            leftSpeed = speed / ratio;
            rightSpeed = speed;
        } else if (curve > 0) {
            double value = MathUtils.log(curve);
            double ratio = (value - 0) / (value + 0);
            if (ratio == 0) {
                ratio = .0000000001;
            }
            leftSpeed = speed;
            rightSpeed = speed / ratio;
        } else {
            leftSpeed = speed;
            rightSpeed = speed;
        } //end borrowing
        this.tankDrive(leftSpeed, rightSpeed); //yay for brilliant hacks!!!
    }

    /*
     * This takes two inputs, left speed and right speed
     * it sets the first two victors
     * 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) {
            FLvictor.set(left);
            FRvictor.set(right);

            if(wheel4) {
                RLvictor.set(left);
                RRvictor.set(right);
            }
        }
    }

    /*
     * This function takes two inputs, magnitude (forward/reverse speed)
     * and rotation (strafe).  It first makes sure that the values aren't
     * greater than 1 and then makes four temporary motor speed values.
     * It then calculates the speed values.  After finding out which motor
     * is running the fastest, it divides the other motor values by the
     * fastest motor speed if its positive.  It then limits the result of
     * fiddling with the speed of each motor.  Finally, it sets the speeds
     * for each motor
     */
    void mecanumDrive(double magnitude, double rotation) { //DONT TOUCH THIS
        if(mecanum) {
            magnitude = limit(magnitude);  //1 is highest, so make n > 1, 1
            rotation  = limit(rotation);   //its not nessecary to do this, but better safe then sorry

            double frontLeftSpeed, rearLeftSpeed, frontRightSpeed, rearRightSpeed;  //temporary motor speed values

            frontLeftSpeed = ((magnitude - rotation));  //calculate speed values
            frontRightSpeed = ((magnitude + rotation));
            rearLeftSpeed = ((magnitude + rotation));
            rearRightSpeed = ((magnitude - rotation));
        
            double maxMotor = Math.max(
                Math.max(Math.abs(frontLeftSpeed), Math.abs(frontRightSpeed)),
                Math.max(Math.abs(rearLeftSpeed), Math.abs(rearRightSpeed))
            );

            if (maxMotor > 1){
                frontRightSpeed = frontRightSpeed / maxMotor;
                rearRightSpeed = rearRightSpeed / maxMotor;
                frontLeftSpeed = frontLeftSpeed / maxMotor;
                rearLeftSpeed = rearLeftSpeed / maxMotor;
            }
        
            frontLeftSpeed = limit(fiddleWithSpeed(frontLeftSpeed));
            frontRightSpeed = limit(fiddleWithSpeed(frontRightSpeed));
            rearLeftSpeed = limit(fiddleWithSpeed(rearLeftSpeed));
            rearRightSpeed = limit(fiddleWithSpeed(rearRightSpeed));

            FLvictor.set(-frontLeftSpeed);  //set the speeds on the motors
            FRvictor.set(frontRightSpeed);  //these four lines could be replaced
            RLvictor.set(-rearLeftSpeed);   //by a set() function, taking in 4
            RRvictor.set(rearRightSpeed);   //doubles and then setting the Victor values
        }

        else {  //If you did not enable the mecanum flag on initilization
            DriverStationLCD lcd;
            lcd = DriverStationLCD.getInstance();
            lcd.println(DriverStationLCD.Line.kMain6, 0, "NO MECANUM");
        }
    } //void checkingDrive

    /*
     * Our hack to turn our mecanum wheels, call it separately
     */
    void turn(double power) {
        if(Math.abs(power) > 0) {
            RRvictor.set(power);
            RLvictor.set(power);
        }
    }

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

    /*
     * 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.
     */
    double fiddleWithSpeed(double input) {
        if(input > 0) {
            input = input * input;
        } else {
            input = -input * input;
        }

        return input;
    }
}

[Jared Russell]

There is nothing that I can see in the code you posted that strikes me as particularly inefficient (I could nitpick things like the overuse of static variables, or the use of a delay inside a function call, but those are style points that shouldn't significantly affect responsiveness).

[Mentor007]

We had a similar problem at FLR. The solution for us was to add a delay/wait (5 or 50 mS) in our camera code. The system was repeatedly calling the camera routine and without the delay it hogged the processing resources to the point that our teleop controls were not getting attention quickly enough, resulting in troublesome delay in robot response for the drive team. A mentor from Team 1511 was instrumental in helping us solve the problem (Thanks Larry). We are Labview based. Hope this is it !

[Jared Russell]

Although you have some camera-related files in your directory, it looks like you are never using them. You might want to (temporarily) delete all of the unneeded files and see if that fixes things.

[TubaMorg]

I would think that your code is the opposite of bloated, it is quite lightweight as a matter of fact.

We use the iterative robot template which I strongly recommend you do. This allows you to do away with the while loop because teleopperiodic is called automatically each robot cycle. In very simple terms, the periodic robot methods are called, then data packets are sent/received, then the periodic method is called again. I am guessing that your while loop is whipping around too often before a packet is sent and the code is getting confused. I do see a delay in your DualShock class that gets called once an iteration, but I think even that is too quick. You aren't seeing these issues in auto, since each command is called a single time and the system is able to process them within cycle times.

If that makes any sense. I am in the middle of a conference right now, so I can't give any more details.

[derekwhite]

Does the delay only happen on the field, or does it happen when testing too?

Field-only lag can be due to that issue with the old routers that just got posted on Bill's Blog, or with USB current issues with USB hubs and the classmate - if you are using a breakout board on the classmate it has to be connected directly, not through an unpowered USB hub.