RobotDemo(void)
{
kicker_in_motion = false;
sol = new Solenoid(2);
rightstick = new Joystick(1);
leftstick = new Joystick(2);
lonelystick = new Joystick (3);
Motor1=new Jaguar(1);
Motor2=new Jaguar(2);
Motor3=new Jaguar(3);
Motor4=new Jaguar(4);
BallGathererMotor9 = new Jaguar(9);
myRobot=new RobotDrive(Motor1,Motor2,Motor4,Motor3);
rlyLED=new Relay(8,Relay::kForwardOnly);
cam = &AxisCamera::GetInstance("10.8.12.11");
cam->WriteResolution(AxisCameraParams::kResolution_160x120);
myRobot->SetExpiration(0.5);
ControllBox = & DriverStation::GetInstance()->GetEnhancedIO();
shooter1 = new Jaguar(5); //front left
shooter2 = new Jaguar(6);
shooter3 = new Jaguar(7);
shooter4 = new Jaguar(8);
//shooterDin = new DigitalInput(1);
shootercontador = new Counter(1);
shootercontador->Start();
shooterspeedTask = new Task("ShooterSpeed",(FUNCPTR)&shooterspeedloop);
kickerTask = new Task ("Kicker", (FUNCPTR)&kickerloop);
Upperlimit = new DigitalInput(3);
Lowerlimit = new DigitalInput(2);
kickermotor = new Relay (6, Relay::kBothDirections);
BridgeBootMotor10 = new Jaguar(10);
kicker_cancel = false;
kicker_down = false;
}
void RobotInit() override {
// create an image
// open the camera at the IP address assigned. This is the IP address that the camera
// can be accessed through the web interface.
camera = new AxisCamera("10.19.67.11");
stick = new jankyXboxJoystick (0);
camera->WriteResolution (AxisCamera::kResolution_320x240);
camera->WriteCompression(30);
camera->WriteRotation(AxisCamera::kRotation_0);
camera->WriteMaxFPS(15);
camera->WriteColorLevel(25);
camera->WriteBrightness(50);
camera->WriteWhiteBalance(AxisCamera::kWhiteBalance_Automatic);
camera->WriteExposureControl(AxisCamera::kExposureControl_Automatic);
}
RobotDemo()
{
#if YEAR_2013
drive = new RobotDrive(left_drive_motor_A_PWM, left_drive_motor_B_PWM,
right_drive_motor_A_PWM, right_drive_motor_B_PWM);
drive->SetExpiration(15);
drive->SetSafetyEnabled(false);
#endif
drive->SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
drive->SetInvertedMotor(RobotDrive::kRearRightMotor, true);
drive->SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
//Joystick
//ds = new DriverStation();
drive_stick_sec = new Joystick(right_stick);
drive_stick_prim = new Joystick(left_stick);
operator_stick = new Joystick(operator_joystick);
//Motors
#if JAGUAR_SWITCH
shooter_motor_front = new Jaguar(shooter_front_motor);
shooter_motor_back = new Jaguar(shooter_back_motor);
#else
shooter_motor_front = new Talon(shooter_front_motor);
shooter_motor_back = new Talon(shooter_back_motor);
#endif
#if DUMB_DRIVE_CODE
left_drive_motor_A = new Victor(left_drive_motor_A_PWM);
left_drive_motor_B = new Victor(left_drive_motor_B_PWM);
right_drive_motor_A = new Victor(right_drive_motor_A_PWM);
right_drive_motor_B = new Victor(right_drive_motor_B_PWM);
#endif
#if YEAR_2013
climbing_motor = new Victor(climbing_motor_PWM);
#endif
//limit switches
top_claw_limit_switch = new DigitalInput(top_claw_limit_switch_port);
bottom_claw_limit_switch = new DigitalInput(
bottom_claw_limit_switch_port);
//Encoders
front_shooter_encoder = new Encoder(shooter_motor_front_encoder_A_port,
shooter_motor_front_encoder_B_port, false);
back_shooter_encoder = new Encoder(shooter_motor_back_encoder_A_port,
shooter_motor_back_encoder_B_port, false);
//solenoids
shooter_angle_1 = new Solenoid(SHOOTER_ANGLE_SOLENOID_1);
shooter_angle_2 = new Solenoid(SHOOTER_ANGLE_SOLENOID_2);
shooter_fire_piston_A = new Solenoid(shooter_fire_piston_solenoid_A);
shooter_fire_piston_B = new Solenoid(shooter_fire_piston_solenoid_B);
//Timers
shooter_piston_timer = new Timer();
VC_timer = new Timer();
loop_time_measure_timer = new Timer();
shooter_reset = new Timer();
pid_code_timer = new Timer();
autonomous_timer = new Timer();
error_timer = new Timer();
retraction_timer = new Timer();
override_timer = new Timer();
stabilizing_timer = new Timer();
shooter_stop_timer = new Timer();
//Compressor
compressor1 = new Compressor(PRESSURE_SWITCH, compressor_enable);
#if CAMERA
//Camera
camera = &(AxisCamera::GetInstance(AxisCameraIpAddr));
camera->WriteResolution(AxisCamera::kResolution_640x480);
AxisCamera::GetInstance();
#endif
//Function starter
compressor1 ->Start();
//float RPS;
//PIDController pid1 (0.1 , 0.001 ,0.0 , &RPS , test_motor );
loop_time_measure_timer ->Start();
VC_timer ->Start();
//Variable Initialization
arcadedrive = true;
test_encoder_value = 0;
total_test_encoder_value = 0;
old_RPS = 0;
new_RPS = 0;
second_count = 0;
set_speed = 0.5;
cycle_counter = 0;
//test_motor ->Set(set_speed);
// float RPS;
//int old_encoder_value;
//double old_timer;
speed2 = 0.0;
test_speed = 0.3;
button = false;
pickup_on = false;
switch_on = false;
kicker_piston_on = false;
kicker_button_on = false;
average_counter = 0;
counter = 1;
number = 1;
additive_error = 0;
prev_error_front = 0;
prev_desired_RPS = 0;
ROC = 0;
claw_ = false;
claw_go_down = false;
constant_ = false;
constant_desired_RPS = false;
divided = 0;
//RPS_encoder_1 = new Encoder(9, 10);// for 2012 robot
shooter_motor_back_RPS = shooter_motor_back->Get();
//RPS_encoder_1 -> SetDistancePerPulse(1 / 250);//TODO figure out how this works
first_press = true;
test_RPS = false;
desired_RPS_control = 0.0;
slow_control = 0;
//RPS_encoder_1 ->Start();
pid_code_timer ->Start();
front_shooter_encoder->Start();
back_shooter_encoder->Start();
autonomous_timer ->Start();
error_timer ->Start();
retraction_timer->Start();
stabilizing_timer->Start();
override_timer->Start();
}
