Autonomous for gears

Technical Java
1

Does this work?

package org.usfirst.frc.team1025.robot.commands.VisionM;

import org.usfirst.frc.team1025.robot.Robot;

import edu.wpi.first.wpilibj.command.Command;

/**
*
*/
public class VisionMa extends Command {
private double centerX;
public VisionMa() {
requires(Robot.visionMaybe);
requires(Robot.chassisSubsystem);
}

// Called just before this Command runs the first time
protected void initialize() {
	Robot.visionMaybe.visionThread.start();

}

// Called repeatedly when this Command is scheduled to run
protected void execute() {
	synchronized (Robot.visionMaybe.imgLock) {
		Robot.visionMaybe.centerX1 = this.centerX;
	}
	double turn = centerX - (Robot.visionMaybe.IMG_WIDTH / 2);
	Robot.chassisSubsystem.robotDrive.arcadeDrive(-0.6, turn * 0.005);
}


// Make this return true when this Command no longer needs to run execute()
public boolean isFinished() {
	if(centerX>0.0)
	return false;
	else if(centerX<237.0){
		return false;
	}
	else{
		return true;
	}
}



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

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

}

2

I’m not sure what you are trying to do with the isFinished() method, but as you have it written right now it will never finish, because centerX will always either be >0 or <237, and the method will return false.

Other than that your logic in execute() looks okay, provided the code that runs in the vision thread works. You may consider using PID to turn the robot instead of feeding values directly into arcade drive method. Hope this helps.

3

Owing to the fact that we are not robots (well, hopefully. At least, I’m not a robot), it’s difficult to tell you if that code “works.” Try to run it on an actual robot, especially before build ends in 1.5 weeks :wink:
You should also try using the debugger (debug as > wpilib java deploy) if you need step-by-step execution of code.

That said,


Robot.visionMaybe.centerX1 = this.centerX;

Did you mean this to be the other way around?


synchronized (Robot.visionMaybe.imgLock) {

synchronized is usually not necessary when you’re just reading values (assuming above point is true)

  • Is this what you wanted in isFinished()?

return centerX <= 0 && centerX >= 237;
  • Also with the above code, it’s not clear whether it’s the behavior you’re actually looking for. Could you explain in words?
  • I’m noticing a lot of magic numbers everywhere. You should make those named constants to make your code readable and clean.
4