/*
* To change this template, choose Tools | Templates
* and open the template in the editor.
*/
package com.grt192.controller.breakaway.teleop;
import com.grt192.core.StepController;
import com.grt192.event.component.EncoderEvent;
import com.grt192.event.component.EncoderListener;
import com.grt192.mechanism.breakaway.GRTBreakawayRobotBase;
import com.grt192.mechanism.GRTDriverStation;
import com.grt192.sensor.GRTEncoder;
import com.grt192.sensor.GRTJoystick;
import com.grt192.utils.Util;
/**
*
* @author Bonan, James, Ryo
*/
public class BreakawayTeleopDriveController extends StepController
implements EncoderListener {
public static final double SPIN_THRESHOLD = .1;
private int driveMode = GRTBreakawayRobotBase.TANK_DRIVE;
private boolean printingData = false;
public BreakawayTeleopDriveController(GRTBreakawayRobotBase rb, GRTDriverStation ds) {
super();
addMechanism("RobotBase", rb);
addMechanism("DriverStation", ds);
((GRTEncoder) rb.getSensor("LeftEncoder")).addEncoderListener(this);
((GRTEncoder) rb.getSensor("RightEncoder")).addEncoderListener(this);
}
public void act() {
GRTDriverStation ds = (GRTDriverStation) getMechanism("DriverStation");
GRTBreakawayRobotBase rb =
(GRTBreakawayRobotBase) getMechanism("RobotBase");
if (ds.getRightButton(9) && driveMode == GRTBreakawayRobotBase.TANK_DRIVE) {
driveMode = GRTBreakawayRobotBase.CAR_DRIVE;
} else if (ds.getRightButton(9) && driveMode == GRTBreakawayRobotBase.CAR_DRIVE){
driveMode = GRTBreakawayRobotBase.TANK_DRIVE;
}
switch (driveMode) {
case GRTBreakawayRobotBase.TANK_DRIVE:
rb.tankDrive(ds.getYLeftJoystick(), -ds.getYRightJoystick());
break;
case GRTBreakawayRobotBase.CAR_DRIVE:
rb.carDrive(ds.getXRightJoystick(),ds.getYRightJoystick(),
Math.abs(ds.getYRightJoystick()) < SPIN_THRESHOLD);
break;
case GRTBreakawayRobotBase.ONE_STICK_DRIVE:
rb.oneStickDrive(
Util.distance(Util.roundValue(
ds.getYLeftJoystick()),
Util.roundValue(ds.getYRightJoystick())),
((GRTJoystick) ds.getSensor("leftJoystick")).getState("JoystickAngle"));
driveMode = GRTBreakawayRobotBase.TANK_DRIVE;
break;
}
if (ds.getLeftButton(6)) {
printingData = !printingData;
System.out.println(printingData ? "printing" : "not pringing");
}
if (printingData) {
printData();
}
}
public void printData() {
GRTBreakawayRobotBase rb =
(GRTBreakawayRobotBase) getMechanism("RobotBase");
System.out.println("gryo: " + rb.getGyroAngle());
System.out.println("x feet: " + rb.getX() / 12.0);
System.out.println("y feet: " + rb.getY() / 12.0);
}
public void countDidChange(EncoderEvent e) {
// System.out.println(e.getSource().getId() + " " +e.getDistance());
}
public void rotationDidStart(EncoderEvent e) {
}
public void rotationDidStop(EncoderEvent e) {
}
public void changedDirection(EncoderEvent e) {
}
}