/* Handles a three motor tribot configuration with outer two wheels driving
* forward and rear wheel rotating backwards for forward motion, and inverse
* for reversing.
*
* Also handles rotation and other requirements
*
* Uses a DifferentialPilot for outer wheels and direct motor control for the
* center rear wheel.
*
*/
package com.cos399.sumo.drive;
import lejos.nxt.MotorPort;
import lejos.nxt.NXTRegulatedMotor;
import lejos.robotics.navigation.DifferentialPilot;
public class Drive {
private static MotorPort leftMotorPort= null, rightMotorPort = null, centerMotorPort = null;
private static NXTRegulatedMotor leftMotor = null, rightMotor = null, centerMotor = null;
private static DifferentialPilot pilot = null;
private static Drive drive = null;
private static int speed, maxRotationSpeed;
private Drive () { }
public static Drive getInstance() {
if (drive == null) {
if (leftMotorPort == null || rightMotorPort == null || centerMotorPort == null)
throw new UnsupportedOperationException("Motor Ports Unassigned");
else {
drive = new Drive();
leftMotor = new NXTRegulatedMotor(leftMotorPort);
rightMotor = new NXTRegulatedMotor(rightMotorPort);
centerMotor = new NXTRegulatedMotor(centerMotorPort);
pilot = new DifferentialPilot(2.2f, 6.25f, leftMotor, rightMotor);
pilot.setRotateSpeed(maxRotationSpeed);
centerMotor.setSpeed(maxRotationSpeed);
Drive.speed = maxRotationSpeed;
}
}
return drive;
}
public static Drive getInstance(MotorPort leftMotor, MotorPort rightMotor,
MotorPort centerMotor, int maxRotationSpeed) {
if (drive == null && (leftMotorPort == null || rightMotorPort == null || centerMotorPort == null)) {
Drive.leftMotorPort = leftMotor;
Drive.rightMotorPort = rightMotor;
Drive.centerMotorPort = centerMotor;
Drive.maxRotationSpeed = maxRotationSpeed;
}
return getInstance();
}
public void setSpeed(int speed) {
pilot.setRotateSpeed(speed);
centerMotor.setSpeed(speed);
}
public int getSpeed() {
return speed;
}
public void setLeftMotorPort(MotorPort port) {
Drive.leftMotorPort = port;
}
public void setRightMotorPort(MotorPort port) {
Drive.rightMotorPort = port;
}
public void setCenterMotorPort(MotorPort port) {
Drive.centerMotorPort = port;
}
public NXTRegulatedMotor getLeftMotor() {
return leftMotor;
}
public NXTRegulatedMotor getRightMotor() {
return rightMotor;
}
public NXTRegulatedMotor getCenterMotor() {
return centerMotor;
}
public void forward() {
pilot.forward();
centerMotor.backward();
}
public void forward(int distance) {
centerMotor.flt();
pilot.travel(distance);
}
public void backward() {
centerMotor.flt();
pilot.backward();
}
public void backward(int distance) {
centerMotor.flt();
pilot.travel(-distance);
}
//positive value rotates left, negative right
public void rotate(int degrees) {
centerMotor.flt();
pilot.rotate(degrees);
}
public void stop(){
pilot.stop();
centerMotor.stop();
}
public int getTachCount() {
return (leftMotor.getTachoCount() + rightMotor.getTachoCount()) / 2;
}
public void resetTach() {
pilot.reset();
centerMotor.resetTachoCount();
}
}