/*
* 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.sensor.GRTMaxBotixSonar;
/**
*
* @author Vikram Sunder
*/
public class Roller extends Mechanism {
public static final double GRAB = -1;
public static final double PUSH = 1;
public static final double STOP = 0;
public static final double CENTER_THRESHOLD = 60;
public static final double SIDE_THRESHOLD = 80;
public Roller(GRTJaguar rollerLeft, GRTJaguar rollerRight,
GRTMaxBotixSonar leftSonar, GRTMaxBotixSonar rightSonar,
GRTMaxBotixSonar centerSonar) {
//starts motors
rollerRight.start();
rollerLeft.start();
//adds Actuators
addActuator("RightRollerMotor", rollerRight);
addActuator("LeftRollerMotor", rollerLeft);
//Adds Sensors
addSensor("LeftSonar", leftSonar);
addSensor("rightSonar", rightSonar);
addSensor("centerSonar", centerSonar);
}
public void setLeftSpeed(double input) {
getActuator("LeftRollerMotor").enqueueCommand(new Command(input));
}
public void setRightSpeed(double input) {
getActuator("RightRollerMotor").enqueueCommand(new Command(input));
}
public void leftGrab() {
setLeftSpeed(-GRAB);
}
public void rightGrab() {
setRightSpeed(GRAB);
}
public void leftPush() {
setLeftSpeed(-PUSH);
}
public void rightPush() {
setRightSpeed(PUSH);
}
public void stopRightRoller() {
setRightSpeed(STOP);
}
public void stopLeftRoller() {
setLeftSpeed(STOP);
}
public boolean leftObstructed(){
return leftRange() <= SIDE_THRESHOLD;
}
public boolean rightObstructed(){
return rightRange() <= SIDE_THRESHOLD;
}
public boolean centerObstructed(){
return centerRange() <= CENTER_THRESHOLD;
}
public double leftRange(){
return getSensor("rightSonar").getState("Value");
}
public double rightRange(){
return getSensor("leftSonar").getState("Value");
}
public double centerRange(){
return getSensor("centerSonar").getState("Value");
}
}