/*
* To change this template, choose Tools | Templates
* and open the template in the editor.
*/
package com.grt192.mechanism.breakaway;
import com.grt192.actuator.GRTJaguar;
import com.grt192.core.Command;
import com.grt192.core.Mechanism;
import com.grt192.event.component.AccelerometerEvent;
import com.grt192.event.component.AccelerometerListener;
import com.grt192.event.component.EncoderEvent;
import com.grt192.event.component.EncoderListener;
import com.grt192.event.component.GyroEvent;
import com.grt192.event.component.GyroListener;
import com.grt192.sensor.GRTAccelerometer;
import com.grt192.sensor.GRTEncoder;
import com.grt192.sensor.GRTGyro;
/**
*
* @author Bonan, James
*/
public class GRTBreakawayRobotBase extends Mechanism implements EncoderListener, GyroListener,
AccelerometerListener{
public static final int TANK_DRIVE = 0;
public static final int ONE_STICK_DRIVE = 1;
public static final int CAR_DRIVE = 2;
public static final double GOAL1X = 2.0;
public static final double GOAL1Y = 0.0;
public static final double GOAL2X = 25;
public static final double GOAL2Y = 0.0;
public final double S_C_BASE_VALUE = 0.5; //Input at which motor starts moving
private boolean automatedDrive = false; // becomes true when it is going
//through a automated sequence (i.e.
//going over bump)
//TODO keep track of position
// private double prevAngle;
private double angle = 0.0;
private double prevLeftDist = 0.0;
private double prevRightDist = 0.0;
private double leftDist = 0.0;
private double rightDist = 0.0;
private double posX = 0.0;
private double posY = 0.0;
public static final double ZONE1STARTX = 0.0; //pre-determined (these are random)
public static final double ZONE1STARTY = 0.0;
public static final double ZONE2STARTX = 7.0;
public static final double ZONE2STARTY = 16.0;
public static final double ZONE3STARTX = 7.0;
public static final double ZONE3STARTY = 53.0;
public static int zone = 1; //starting zone
private double gyroWarmUpSum = 0.0;
private int gyroCount = 0;
public final double GYRO_TOTAL_WARM_UP_COUNT = 200.0;
private double gyroWarmUpAverage = 0.0;
private double prevAngle;
private double axWarmUpSum = 0.0;
private int axCount = 0;
public final double AX_TOTAL_WARM_UP_COUNT = 200.0;
private double axWarmUpAverage = 0.0;
//the entire robot base, including input and output...
public GRTBreakawayRobotBase(GRTJaguar leftJaguar1, GRTJaguar leftJaguar2,
GRTJaguar rightJaguar1, GRTJaguar rightJaguar2,
GRTAccelerometer ax1, GRTGyro gy1,
GRTEncoder leftEn, GRTEncoder rightEn) {
leftJaguar1.start();
leftJaguar2.start();
rightJaguar1.start();
rightJaguar2.start();
ax1.start();
gy1.start();
gy1.addGyroListener(this);
leftEn.start();
//leftEn.addEncoderListener(this);
rightEn.start();
//rightEn.addEncoderListener(this);
addActuator("LeftJaguar1", leftJaguar1);
addActuator("LeftJaguar2", leftJaguar2);
addActuator("RightJaguar1", rightJaguar1);
addActuator("RightJaguar2", rightJaguar2);
addSensor("AccelerometerX", ax1);
addSensor("Gyro", gy1);
addSensor("LeftEncoder", leftEn);
addSensor("RightEncoder", rightEn);
if (zone == 1) {
posX = ZONE1STARTX;
posY = ZONE1STARTY;
}
else if (zone == 2) {
posX = ZONE2STARTX;
posY = ZONE2STARTY;
}
else if (zone == 3) {
posX = ZONE3STARTX;
posY = ZONE3STARTY;
}
}
public double getAccelerationX() {
return getSensor("AccelerometerX").getState("Acceleration");
}
public double getLeftDist() {
return ((GRTEncoder) getSensor("LeftEncoder")).getState("Distance");
}
public double getRightDist() {
return ((GRTEncoder) getSensor("RightEncoder")).getState("Distance");
}
public double getGyroAngle() {
return getSensor("Gyro").getState("Angle");
}
public double getAngle() {
return angle;
}
public void overrideAutomated() {
automatedDrive = false;
}
public void tankDrive(double leftValue, double rightValue) {
// the negative sign is for making it so that the joystick isn't flipped
if (!automatedDrive) {
Command c1 = new Command(leftValue, 0);
Command c2 = new Command(rightValue, 0);
getActuator("LeftJaguar1").enqueueCommand(c1);
getActuator("LeftJaguar2").enqueueCommand(c1);
getActuator("RightJaguar1").enqueueCommand(c2);
getActuator("RightJaguar2").enqueueCommand(c2);
}
}
public void carDrive(double x, double y) {
carDrive(x, y, false);
}
public void carDrive(double x, double y, boolean spin) {
double leftSpeed = x;
double rightSpeed = x;
if (y < 0) {
leftSpeed *= (spin? 1/x : -1) * y;
} else if (y > 0) {
rightSpeed *= (spin? -1/x : 1) * y;
}
tankDrive(leftSpeed, -rightSpeed);
}
public void oneStickDrive(double magnitude, double angle) { //point turn
if (!automatedDrive) {
double turnAngle = turn(angle, getAngle());
Command c1;
Command c2;
double positiveSpeed = S_C_BASE_VALUE + (1 - S_C_BASE_VALUE) * magnitude;
double negativeSpeed = S_C_BASE_VALUE + (1 - S_C_BASE_VALUE) * magnitude;
if (turnAngle > 0) {
c1 = new Command(positiveSpeed, 0);
c2 = new Command(negativeSpeed, 0);
} else {
if (turnAngle < 0) {
c1 = new Command(negativeSpeed, 0);
c2 = new Command(positiveSpeed, 0);
} else {
c1 = new Command(0, 0);
c2 = new Command(0, 0);
}
}
getActuator("LeftJaguar1").enqueueCommand(c1);
getActuator("LeftJaguar2").enqueueCommand(c1);
getActuator("RightJaguar1").enqueueCommand(c2);
getActuator("RightJaguar2").enqueueCommand(c2);
}
}
public void overTheBump() {
automatedDrive = true;
Command c1 = new Command(1, 0);
Command c2 = new Command(1, 0);
getActuator("LeftJaguar1").enqueueCommand(c1);
getActuator("LeftJaguar2").enqueueCommand(c1);
getActuator("RightJaguar1").enqueueCommand(c2);
getActuator("RightJaguar2").enqueueCommand(c2);
try {
Thread.sleep(1000);
} catch (InterruptedException ex) {
ex.printStackTrace();
}
if (automatedDrive) {
Command c3 = new Command(1, 0);
Command c4 = new Command(1, 0);
getActuator("LeftJaguar1").enqueueCommand(c3);
getActuator("LeftJaguar2").enqueueCommand(c3);
getActuator("RightJaguar1").enqueueCommand(c4);
getActuator("RightJaguar2").enqueueCommand(c4);
try {
Thread.sleep(1000);
} catch (InterruptedException ex) {
ex.printStackTrace();
}
}
if (automatedDrive) {
Command c3 = new Command(1, 0);
Command c4 = new Command(1, 0);
getActuator("LeftJaguar1").enqueueCommand(c3);
getActuator("LeftJaguar2").enqueueCommand(c3);
getActuator("RightJaguar1").enqueueCommand(c4);
getActuator("RightJaguar2").enqueueCommand(c4);
try {
Thread.sleep(1000);
} catch (InterruptedException ex) {
ex.printStackTrace();
}
}
automatedDrive = false;
}
public void updatePos() //angle in degrees, airplane style (this is how the gyro is)
{
posX += Math.sin(angle * Math.PI / 180.0) * ((leftDist - prevLeftDist) +
(rightDist - prevRightDist)) / 2.0;
posY += Math.cos(angle * Math.PI / 180.0) * ((leftDist - prevLeftDist) +
(rightDist - prevRightDist)) / 2.0;
}
private static double turn(double start, double end) {
end -= start; //shift start to 0, and end accordingly
end += end < 0 ? 360 : 0; //make end angle positve
return end > 180 ? end - 360 : end; //pick the shorter side
}
public void countDidChange(EncoderEvent e) {
}
public double getX() //x position on field
{
return posX;
}
public double getY() //y position on field
{
return posY;
}
public void rotationDidStart(EncoderEvent e) {
}
public void rotationDidStop(EncoderEvent e) {
}
public void changedDirection(EncoderEvent e) {
}
public void didReceiveAngle(GyroEvent e) {
}
public void didAngleChange(GyroEvent e) {
}
public void didAngleSpike(GyroEvent e) {
}
public void didReceiveAcceleration(AccelerometerEvent e) {
}
public void didAccelerationSpike(AccelerometerEvent e) {
}
public void didAccelerationChange(AccelerometerEvent e) {
}
}