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 RobotInit()
{
// Auto chooser
chooser = new SendableChooser();
chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
SmartDashboard::PutData("Auto Modes", chooser);
// Drive declarations
frontLeftTalon = new CANTalon(frontLeftChannel);
rearLeftTalon = new CANTalon(rearLeftChannel);
frontRightTalon = new Victor(frontRightChannel);
rearRightTalon = new CANTalon(rearRightChannel);
//frontLeftTalon->SetInverted(true);
rearLeftTalon->SetInverted(true);
yawGyro = new ADXRS450_Gyro();
robotDrive = new RobotDrive(frontLeftTalon, rearLeftTalon, frontRightTalon, rearRightTalon);
flightStick = new Joystick(flightstickChannel);
robotDrive -> SetSafetyEnabled(false);
// Shooter variable declarations MAKE SURE YOU PLUG THEM INTO THE RIGHT PORTS WENDY
motor1 = new Talon(8); // loook above
motor2 = new Jaguar(4);
motor3 = new Jaguar(5);
shootStick = new Joystick(0);
shooterSwitch = new DigitalInput(0);
// Distance sensor declarations
distanceSensor = new AnalogInput(0);
getVcc = new AnalogInput(2);
// Servo
doorLift = new Servo(9);
// Camera stuff
camera1 = new USBCamera("cam0", false);
camera2 = new USBCamera("cam1", false);
camera1->OpenCamera();
camera2->OpenCamera();
camera1->StartCapture();
camera2->StartCapture();
//Camera servos
frontCamServo = new Servo(10); // WHAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAT
backCamServo = new Servo(11); // WHAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAT
}