Robot() :
robotDrive(Motor1, Motor2), // these must be initialized in the same order
stick(5), // as they are declared above.
lw(LiveWindow::GetInstance()),
autoLoopCounter(0),
Motor1(21),
Motor2(12),
Slave1(20),
Slave2(14),
t_motor(13),
arm_Motor(23),
finger_Motor(22),
intake_Spin_Motor(11),
intake_Winch_Motor(13),
stick2(4),
autoLoopCounter2(0)
{
robotDrive.SetExpiration(0.1);
robotDrive.SetSafetyEnabled(false);
Slave1.SetControlMode(CANSpeedController::kFollower);
Slave1.Set(21);
Slave2.SetControlMode(CANSpeedController::kFollower);
Slave2.Set(12);
Motor2.SetInverted(true); //12
Slave2.SetInverted(true);//14
arm_Motor.SetInverted(false);//23
t_motor.SetInverted(true);//23
// t_motor.SetControlMode(CANSpeedController::kVoltage);
// t_motor.Set(0);
// CameraServer::GetInstance()->SetQuality(50);
// CameraServer::GetInstance()->SetSize(2);
// //the camera name (ex "cam0") can be found through the roborio web interface
// CameraServer::GetInstance()->StartAutomaticCapture("cam0");
t_motor.SetControlMode(CANSpeedController::kPercentVbus);
// t_motor.SetVoltageCompensationRampRate(24.0);
t_motor.SetFeedbackDevice(CANTalon::QuadEncoder);
t_motor.SetPosition(0);
// t_motor.SetPID(1, 0, 0);
arm_Motor.SetControlMode(CANSpeedController::kPercentVbus);
finger_Motor.SetControlMode(CANSpeedController::kPercentVbus);
// ourRangefinder = new AnalogInput(0);
}
void OperatorControl()
{
while (IsOperatorControl() && IsEnabled())
{
robotDrive.ArcadeDrive(scaler(stick.GetZ()),scaler(stick.GetY()));
SmartDashboard::PutNumber("StickZ",stick.GetZ());
SmartDashboard::PutNumber("StickZscaled",scaler(stick.GetZ()));
finger_Motor.Set(scaler(stick2.GetRawAxis(thumbpadL_Y)));
// scalerValue = stick.GetRawAxis(3);
arm_Motor.Set(scaler(stick2.GetRawAxis(thumbpadR_Y)));
manualShooter();
shootingModes();
toggleIntake();
toggleIntakeMode();
setScalerValue();
// double volts = ourRangefinder->GetVoltage();
// SmartDashboard::PutNumber("Voltage",volts);
//t_motor.Set(stick2.GetZ());
//t_motor.Set(stick.GetAxis(Joystick::kDefaultThrottleAxis));
// t_motor.Set(stick.GetAxis(Joystick::kThrottleAxis));
// finger_Motor.Set(stick2.GetAxis(Joystick::kThrottleAxis));
// arm_Motor.Set(stick2.GetY());
// Current Control mode Debug
SmartDashboard::PutNumber("Motor30 Current",t_motor.GetOutputCurrent());
SmartDashboard::PutNumber("Position",t_motor.GetPosition());
// t_motor.Set(stick.GetAxis(Joystick::Slider));
// if (stick.GetRawButton(3) == true){
// t_motor.SetPosition(10000);
// }
// if (stick2.GetRawButton(7) == true and buttonpress == false){
//
// }
// else if (stick2.GetRawButton(3) == false and buttonpress == true){
// buttonpress = false;// drive with arcade style (use right stick)
//
// }
SmartDashboard::PutBoolean("buttonpress state",buttonpress);
if (stick2.GetRawButton(buttonBack) == true){
stateDisarmed = true;
stateArming1 = false;
stateArming2 = false;
stateArmed = false;
stateFiring1 = false;
stateFiring2 = false;
t_motor.SetPosition(0);
}
toggleIntake();
// if (stick2.GetRawButton(5) == true){
// intake_Spin_Motor.Set(1);
// }
// if (stick2.GetRawButton(5) == false){
// intake_Spin_Motor.Set(0);
// }
if (stick.GetRawButton(2) == true and buttonpress2 == false){
myServo->SetAngle(175);
}
if (stick.GetRawButton(2) == false and buttonpress2 == true){
myServo->SetAngle(5);
}
// servoSetPos(servoState);
// myServo->Set(.5);
Wait(0.005);// wait for a motor update time
// motorSetPos(launcherState);
}
}