package edu.wpi.first.wpilibj.templates.subsystems;
import edu.wpi.first.wpilibj.PWM;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.DriveTrainCommand;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
*
* @author ming
*/
public class DriveTrainSubsystem extends Subsystem {
PWM leftMotorUno = new PWM(RobotMap.leftMotorUno);
PWM leftMotorDue = new PWM(RobotMap.leftMotorDue);
PWM rightMotorUno = new PWM(RobotMap.rightMotorUno);
PWM rightMotorDue = new PWM(RobotMap.rightMotorDue);
//TODO FIND GOOD VALUES FOR THESE
//PLACE HOLDER
//PLACE HOLDER
/* The following return the speed of the motors
* between -1 and 1, NOT THE RAW VALUE.
*/
public double getLeftMotor() {
return leftMotorUno.getSpeed(); //motors on same side should be same speed
}
public double getRightMotor() {
return rightMotorUno.getSpeed();
}
protected void initDefaultCommand() {
//TODO finish drive train
setDefaultCommand(new DriveTrainCommand()); //should run on start
}
/*
* set the speed of the left motor
*/
public void setLeftMotor(int value) {
Debug.print("Setting left motor to " + value + " ");
SmartDashboard.putInt("Left Motor PWM", value);
leftMotorUno.setRaw(255 - value);
leftMotorDue.setRaw(255 - value);
}
/*
* set the speed of the right motor
*/
public void setRightMotor(int value) {
Debug.print("Setting right motor " + value + " ");
SmartDashboard.putInt("Right Motor PWM", value);
rightMotorUno.setRaw(255 - value); //reverse
rightMotorDue.setRaw(255 - value);
}
}