/*
* To change this template, choose Tools | Templates
* and open the template in the editor.
*/
package com.grt192.controller.breakaway.auto;
import com.grt192.core.StepController;
import com.grt192.mechanism.GRTDriverStation;
import com.grt192.mechanism.breakaway.GRTBreakawayRobotBase;
/**
*
* @author grtstudent
*/
public class BreakAwayRobotPositioningController extends StepController { //THIS CLASS IS ONLY FOR SALVAGING CODE
//where information about the robot (position, angle, bump etc) is upadated.
private double posX = 0.0;
private double posY = 0.0;
public static final double ZONE1STARTX = 7.0; //pre-determined (these are estimates)
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 static final double DT = SLEEP_INTERVAL / 1000.0; //sec
private double warmUpSum = 0.0;
private double count = 0.0;
public final double TOTAL_WARM_UP_COUNT = 200.0;
private double warmUpAverage = 0.0;
private double prevAngle;
private double prevLeftDist = 0.0;
private double prevRightDist = 0.0;
public BreakAwayRobotPositioningController(GRTBreakawayRobotBase rb) {
super();
addMechanism("RobotBase", rb);
if (zone == 1) {
posX = ZONE1STARTX;
posY = ZONE1STARTY;
}
if (zone == 2) {
posX = ZONE2STARTX;
posY = ZONE2STARTY;
}
if (zone == 3) {
posX = ZONE3STARTX;
posY = ZONE3STARTY;
}
}
public void act() {
GRTBreakawayRobotBase rb = (GRTBreakawayRobotBase) getMechanism("RobotBase");
double angle = rb.getAngle();
double leftDist = rb.getLeftDist();
double rightDist = rb.getRightDist();
count++;
if (count <= TOTAL_WARM_UP_COUNT) {
warmUpSum += angle - prevAngle;
prevAngle = angle;
System.out.println("Calibrating: "
+ (count / TOTAL_WARM_UP_COUNT * 100.0)
+ "% done");
if(count == TOTAL_WARM_UP_COUNT) {
warmUpAverage = warmUpSum / TOTAL_WARM_UP_COUNT;
}
} else {
angle -= warmUpAverage * count; //offset not constant
System.out.println("angle: " + angle);
}
updatePos(leftDist, rightDist, angle);
prevLeftDist = leftDist;
prevRightDist = rightDist;
System.out.println("x: " + posX);
System.out.println("y " + posY);
}
public void updatePos(double leftDist, double rightDist, double angle) //angle in degrees, math style
{
posX += Math.cos(angle * Math.PI / 180.0) * ((leftDist - prevLeftDist) +
(rightDist - prevRightDist)) / 2.0;
posY += Math.sin(angle * Math.PI / 180.0) * ((leftDist - prevLeftDist) +
(rightDist - prevRightDist)) / 2.0;
// GRTBreakawayRobotBase rb = (GRTBreakawayRobotBase) getMechanism("RobotBase");
// rb.updatePos(posX, posX, angle);
}
}