[cpapplefamily]

We have been exploring some code options most recent the PIDCommand. We have a PIDCommand [Drive_Spin_In_Place_PID] that is created when a button on our joystick is pressed and the command [x_Test_Lookup] creates the [Drive_Spin_In_Place_PID] and starts the command. In an effort to learn what is happening we are printing the value of Drive_Spin_In_Place_PID_count and see that the number increases each time we press the button. This follows what we might expect since the PIDCommand description tells us its a command that contains it's own PID loop for short lived PID uses. I wonder can we over do it burning up memory? Do the old objects PID's created from previous button presses still exist or are they removed for garbage collection? We not Java experts but learning a lot along the way. There are some admittedly parts of the code that don't work and need to be worked through.

public class x_Test_Lookup extends Command {
	
	Drive_Spin_In_Place_PID spinToTarget_Command ;
    public x_Test_Lookup() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	requires(Robot.driveTrain);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    	
    	double cX = Robot.vision.my_Get_Xcenter();
    	double spin = Robot.vision.my_SpinToTarget_Lookup(cX);
    	System.out.println("spin = " + spin);
    	spinToTarget_Command = new Drive_Spin_In_Place_PID(spin*-1,false);
    	spinToTarget_Command.start();
    	
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	//double a = Robot.vision.my_Get_Xcenter();
    	//SmartDashboard.putNumber("ANGLE_TO_TARGET",Robot.vision.my_SpinToTarget_Lookup(a));
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return false;
    }

    // Called once after isFinished returns true
    protected void end() {
    	spinToTarget_Command.end();
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	end();
    }
}

public class Drive_Spin_In_Place_PID extends PIDCommand {
	double m_setpoint;
	private int scancount = 0;
	static int Drive_Spin_In_Place_PID_count = 0;
    public Drive_Spin_In_Place_PID(double setpoint,boolean absoult) {

    	
        super("Gyro_PID_Spin"+Drive_Spin_In_Place_PID_count, .04, 0.001, 0.0000, 0.02); //0.00002, 0.000002, 0.02);//, 0.00005, 0.0002, 0.02);
        getPIDController().setContinuous(false);
        getPIDController().setAbsoluteTolerance(.5);
        getPIDController().setOutputRange(-0.75, 0.75);
      
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
        requires(Robot.driveTrain);
	
        if(absoult){
        	System.out.println("absoult"+Drive_Spin_In_Place_PID_count);
        	m_setpoint = setpoint;
        }else{
        	System.out.println("Not absoult"+Drive_Spin_In_Place_PID_count);
        	m_setpoint = Robot.driveTrain.my_Get_Gyro() + setpoint;
        }
        Drive_Spin_In_Place_PID_count++;
       
    }

    protected double returnPIDInput() {
        // Return your input value for the PID loop
        // e.g. a sensor, like a potentiometer:
        // yourPot.getAverageVoltage() / kYourMaxVoltage
    	
        return Robot.driveTrain.my_Get_Gyro();      
    }

    protected void usePIDOutput(double output) {
        // Use output to drive your system, like a motor
        // e.g. yourMotor.set(output);
    	if(timeSinceInitialized() > Robot.vision.light_Settle_Time){
    		
    		Robot.driveTrain.my_Drive_Mecanum(0,0, output, 0);
    	}
       
    }
    

    // Called just before this Command runs the first time
    protected void initialize() {
    	m_setpoint = Robot.driveTrain.my_Get_Gyro() + m_setpoint;
    	getPIDController().enable();
    	getPIDController().setSetpoint(m_setpoint);
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
    	double gyro = Robot.driveTrain.my_Get_Gyro();
    	
    	if(gyro>m_setpoint-.5 && gyro< m_setpoint +.5){
    		scancount = scancount+1;
    		if(scancount>10){
    			return true;
    		}else{
    			System.out.println("Scancount = "+scancount);
    			return false;
    		}
    	}else{
    		scancount = 0;
    		return false;
    	}
    }

    // Called once after isFinished returns true
    protected void end() {
    	Robot.driveTrain.my_Drive_Mecanum(0,0,0,0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	end();
    }
}