Robot Control with using PID andGeartooth Sensor on Java

Technical Programming
1

Hello. I have been working on autonomous period with using PID and Geartooth.

I would like to describe my problem. My first problem is that I want to move robot forward. For example: Forward degree is 100. If I hit the robot, I want to turn the robot to 100 degree without using Gyro.

How can I do that with PID and Geartooth Sensor? I have an example code but didn’t work. It still doesn’t move forward.

I can’t try the code until our competition day. How should I write the code?

Thanks.

public class Robot extends IterativeRobot {
	private static final String kDefaultAuto = "Default";
	private static final String kCustomAuto = "My Auto";
	private String m_autoSelected;
	private SendableChooser<String> m_chooser = new SendableChooser<>();
	Spark leftMotor = new Spark(0);
	Spark rightMotor= new Spark(7);
	Joystick stick = new Joystick(0);
	Counter leftCount= new Counter(5);
	Counter rightCount= new Counter(6);
	PIDController leftControl= new PIDController(0.1,0,0, leftCount ,leftMotor);
	PIDController rightControl= new PIDController(0.1,0,0, rightCount ,rightMotor);



	
	public void robotInit() {
		m_chooser.addDefault("Default Auto", kDefaultAuto);
		m_chooser.addObject("My Auto", kCustomAuto);
		SmartDashboard.putData("Auto choices", m_chooser);
		leftCount.setPIDSourceType(PIDSourceType.kDisplacement);
		rightCount.setPIDSourceType(PIDSourceType.kDisplacement);
		
		leftCount.setReverseDirection(true);
		rightMotor.setInverted(true);
		leftCount.reset();
		rightCount.reset();
		
		leftControl.setOutputRange(-0.75, 0.75);
		rightControl.setOutputRange(-0.75, 0.75);
		
		leftControl.enable();
		rightControl.enable(); 


	}

	
	public void autonomousInit() {
		m_autoSelected = m_chooser.getSelected();
		// autoSelected = SmartDashboard.getString("Auto Selector",
		// defaultAuto);
		System.out.println("Auto selected: " + m_autoSelected);
	}

	
	public void autonomousPeriodic() {
		
		leftControl.setSetpoint(10);
		rightControl.setSetpoint(10);
		
		
		
	}
	
	public void teleopInit()
	{

	leftControl.disable();
	rightControl.disable();
	}

	
	public void teleopPeriodic() {
	}

	public void testPeriodic() {
	}
2