package com.grt192.mechanism.cannonbot;
import com.grt192.actuator.GRTCANJaguar;
import com.grt192.controller.pid.AsynchronousPIDController;
import com.grt192.core.Mechanism;
import com.grt192.event.component.GyroEvent;
import com.grt192.event.component.GyroListener;
import com.grt192.event.component.JagEncoderEvent;
import com.grt192.event.component.JagEncoderListener;
import com.grt192.event.controller.PIDEvent;
import com.grt192.event.controller.PIDListener;
import com.grt192.sensor.GRTGyro;
import com.grt192.sensor.canjaguar.GRTJagEncoder;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
/**
*
* @author anand
*
*/
public class CBTankDriveTrain extends Mechanism implements PIDOutput,
JagEncoderListener, GyroListener, PIDListener {
public static final int WAIT_INTERVAL = 2;
public static final int ROBOT_WIDTH = 28; // TODO find
private double turnP = .10;
private double turnI = .10;
private double turnD = .10;
private double driveP = .10;
private double driveI = .10;
private double driveD = .10;
private GRTCANJaguar leftJaguar;
private GRTCANJaguar rightJaguar;
private GRTJagEncoder leftEncoder;
private GRTJagEncoder rightEncoder;
private GRTGyro canGyro;
private double leftWheelX;
private double leftWheelY;
private double rightWheelX;
private double rightWheelY;
private boolean pointTurn;
private AsynchronousPIDController turnControl;
private AsynchronousPIDController leftDriveControl;
private AsynchronousPIDController rightDriveControl;
public CBTankDriveTrain(int lfpin, int rfpin, int gyropin) {
this(new GRTCANJaguar(lfpin), new GRTCANJaguar(rfpin), new GRTGyro(
gyropin, 5, "cangyro"));
}
public CBTankDriveTrain(GRTCANJaguar lfjaguar, GRTCANJaguar rfjaguar,
GRTGyro canGyro) {
this.leftJaguar = lfjaguar;
leftJaguar.start();
this.rightJaguar = rfjaguar;
rightJaguar.start();
this.leftEncoder = new GRTJagEncoder(lfjaguar, 5, "ljagencoder");
this.rightEncoder = new GRTJagEncoder(rfjaguar, 5, "rjagencoder");
this.canGyro = canGyro;
leftEncoder.addEncoderListener(this);
rightEncoder.addEncoderListener(this);
canGyro.addGyroListener(this);
canGyro.start();
leftEncoder.start();
rightEncoder.start();
turnControl = new AsynchronousPIDController(new PIDController(turnP,
turnI, turnD, canGyro, this));
leftDriveControl = new AsynchronousPIDController(new PIDController(
driveP, driveI, driveD, leftEncoder, this));
rightDriveControl = new AsynchronousPIDController(new PIDController(
driveP, driveI, driveD, rightEncoder, this));
pointTurn = true;
leftDriveControl.addPIDListener(this);
rightDriveControl.addPIDListener(this);
turnControl.addPIDListener(this);
leftDriveControl.start();
rightDriveControl.start();
turnControl.start();
leftWheelX = -ROBOT_WIDTH / 2;
rightWheelX = ROBOT_WIDTH / 2;
leftWheelY = rightWheelY = 0;
addActuator("lfJaguar", leftJaguar);
addActuator("rbJaguar", rightJaguar);
addSensor("ljagencoder", leftEncoder);
addSensor("rjagencoder", rightEncoder);
addSensor("cangyro", canGyro);
}
public double getDriveP() {
return driveP;
}
public void setDriveP(double driveP) {
this.driveP = driveP;
}
public double getDriveI() {
return driveI;
}
public void setDriveI(double driveI) {
this.driveI = driveI;
}
public double getDriveD() {
return driveD;
}
public void setDriveD(double driveD) {
this.driveD = driveD;
}
/**
* Drive dts, specifying speed
*
* @param leftSpeed
* @param rightSpeed
*/
public void tankDrive(double leftSpeed, double rightSpeed) {
this.leftJaguar.enqueueCommand(leftSpeed);
this.rightJaguar.enqueueCommand(rightSpeed);
}
public void driveToPosition(double position) {
driveToPosition(position, position);
}
/**
* Drive dts to a specific position, provided where the l&r wheels should be
*
* @param leftPosition
* @param rightPosition
*/
public void driveToPosition(double leftPosition, double rightPosition) {
stopPIDControl();
leftDriveControl.getPidThread().setSetpoint(leftPosition);
rightDriveControl.getPidThread().setSetpoint(rightPosition);
leftDriveControl.enable();
rightDriveControl.enable();
}
public void driveDistance(double distance) {
driveDistance(distance, distance);
}
/**
* Drive l&r wheels a certain distance
*
* @param leftDistance
* @param rightDistance
*/
public void driveDistance(double leftDistance, double rightDistance) {
driveToPosition(leftEncoder.getState("Distance") + leftDistance,
rightEncoder.getState("Distance") + rightDistance);
}
public void turnTo(double angle) {
turnTo(angle, true);
}
/**
* Turn the robot to a specific heading
*
* @param angle
* @param point
*/
public void turnTo(double angle, boolean point) {
stopPIDControl();
pointTurn = point;
turnControl.getPidThread().setSetpoint(angle);
turnControl.enable();
}
public void pidWrite(double output) {
// Scale speed control to output ratio
if (pointTurn) {
tankDrive(output, -output);
} else {
tankDrive((1 + output) / 2, 1 - (1 + output) / 2);
}
}
/**
* Stop the driveTrains
*/
public void stop() {
stopPIDControl();
tankDrive(0, 0);
}
public double getTurnP() {
return turnP;
}
public void setTurnP(double turnP) {
this.turnP = turnP;
}
public double getTurnI() {
return turnI;
}
public void setTurnI(double turnI) {
this.turnI = turnI;
}
public double getTurnD() {
return turnD;
}
public void setTurnD(double turnD) {
this.turnD = turnD;
}
public boolean isPointTurn() {
return pointTurn;
}
public void setPointTurn(boolean pointTurn) {
this.pointTurn = pointTurn;
}
private void block() {
try {
Thread.sleep(WAIT_INTERVAL);
} catch (InterruptedException e) {
e.printStackTrace();
}
}
public double getLeftPosition() {
return leftEncoder.getState("Distance");
}
public double getRightPosition() {
return rightEncoder.getState("Distance");
}
public double getHeading() {
return canGyro.getState("Angle");
}
public void changedDirection(JagEncoderEvent e) {
// TODO Auto-generated method stub
}
public void countDidChange(JagEncoderEvent e) {
double deltaR = e.getDistance() - e.getSource().getState("Previous");
if (e.getSource().getId().equals("ljagencoder")) {
// left wheel
leftWheelX = deltaR * Math.cos(Math.toRadians(getHeading()));
leftWheelY = deltaR * Math.sin(Math.toRadians(getHeading()));
} else if (e.getSource().getId().equals("rjagencoder")) {
// right wheel
rightWheelX = deltaR * Math.cos(Math.toRadians(getHeading()));
rightWheelY = deltaR * Math.sin(Math.toRadians(getHeading()));
}
}
public void rotationDidStart(JagEncoderEvent e) {
// TODO Auto-generated method stub
}
public void rotationDidStop(JagEncoderEvent e) {
// TODO Auto-generated method stub
}
public void didAngleChange(GyroEvent e) {
}
public void didAngleSpike(GyroEvent e) {
// TODO Auto-generated method stub
}
public void didReceiveAngle(GyroEvent e) {
// TODO Auto-generated method stub
}
public double getLeftWheelX() {
return leftWheelX;
}
public double getLeftWheelY() {
return leftWheelY;
}
public double getRightWheelX() {
return rightWheelX;
}
public double getRightWheelY() {
return rightWheelY;
}
public double getX() {
return (getLeftWheelX() + getRightWheelX()) / 2;
}
public double getY() {
return (getRightWheelY() + getRightWheelY()) / 2;
}
public void stopPIDControl() {
leftDriveControl.reset();
rightDriveControl.reset();
turnControl.reset();
}
public void suspendPIDControl() {
leftDriveControl.disable();
rightDriveControl.disable();
turnControl.disable();
}
public void onSetpointReached(PIDEvent e) {
e.getSource().reset();
}
}