// RobotBuilder Version: 0.0.2
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// C++ from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in th future.
#include "RobotMap.h"
#include "LiveWindow/LiveWindow.h"
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=ALLOCATION
Compressor* RobotMap::airCompressorAirCompressor = NULL;
Solenoid* RobotMap::driveTrainLeftShifter = NULL;
Solenoid* RobotMap::driveTrainRightShifter = NULL;
SpeedController* RobotMap::driveTrainLeftMotors1and2 = NULL;
SpeedController* RobotMap::driveTrainLeftMotor3 = NULL;
SpeedController* RobotMap::driveTrainRightMotors1and2 = NULL;
SpeedController* RobotMap::driveTrainRightMotor3 = NULL;
RobotDrive* RobotMap::driveTrainRobotDrive = NULL;
Encoder* RobotMap::driveTrainQuadratureEncoderLeft = NULL;
Encoder* RobotMap::driveTrainQuadratureEncoderRight = NULL;
SpeedController* RobotMap::shooterWheelPIDControllerShooterMotor = NULL;
Encoder* RobotMap::shooterWheelPIDControllerShooterSpeedEncoder = NULL;
SpeedController* RobotMap::shooterShooterTiltMotor = NULL;
Encoder* RobotMap::shooterShooterTiltEncoder = NULL;
GearTooth* RobotMap::shooterShooterTiltHomeSensor = NULL;
Solenoid* RobotMap::shooterLatchFrisbeeSolenoid = NULL;
Solenoid* RobotMap::shooterLoadFrisbeeSolenoid = NULL;
Solenoid* RobotMap::shooterShootSolenoid = NULL;
SpeedController* RobotMap::frisbeeStorageFrisbeeIndexerMotor = NULL;
Encoder* RobotMap::frisbeeStorageFrisbeeIndexerSpeedEncoder = NULL;
DigitalInput* RobotMap::frisbeeStorageFrisbeeInputOpticalSensor = NULL;
DigitalInput* RobotMap::frisbeeStorageFrisbeeIndexOpticalSensor = NULL;
DigitalInput* RobotMap::frisbeeStorageFrisbeeReadytoshootOpticalSensor = NULL;
SpeedController* RobotMap::frisbeePickupIntakeandCenteringRollerMotors = NULL;
SpeedController* RobotMap::pickupArmPickupArmMotor = NULL;
Encoder* RobotMap::pickupArmPickupArmEncoder = NULL;
PIDController* RobotMap::pickupArmPickupArmPIDController = NULL;
GearTooth* RobotMap::pickupArmPickupArmHomeSensor = NULL;
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=ALLOCATION
void RobotMap::init() {
	// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
	LiveWindow* lw = LiveWindow::GetInstance();
	airCompressorAirCompressor = new Compressor(1, 5, 1, 1);
	
	
	driveTrainLeftShifter = new Solenoid(1, 1);
	lw->AddActuator("Drive Train", "Left Shifter", driveTrainLeftShifter);
	
	driveTrainRightShifter = new Solenoid(1, 2);
	lw->AddActuator("Drive Train", "Right Shifter", driveTrainRightShifter);
	
	driveTrainLeftMotors1and2 = new Talon(1, 5);
	lw->AddActuator("Drive Train", "Left Motors 1 and 2", (Talon*) driveTrainLeftMotors1and2);
	
	driveTrainLeftMotor3 = new Talon(1, 6);
	lw->AddActuator("Drive Train", "Left Motor 3", (Talon*) driveTrainLeftMotor3);
	
	driveTrainRightMotors1and2 = new Talon(1, 2);
	lw->AddActuator("Drive Train", "Right Motors 1 and 2", (Talon*) driveTrainRightMotors1and2);
	
	driveTrainRightMotor3 = new Talon(1, 3);
	lw->AddActuator("Drive Train", "Right Motor 3", (Talon*) driveTrainRightMotor3);
	
	driveTrainRobotDrive = new RobotDrive(driveTrainLeftMotors1and2, driveTrainLeftMotor3,
              driveTrainRightMotors1and2, driveTrainRightMotor3);
	
	driveTrainRobotDrive->SetSafetyEnabled(true);
        driveTrainRobotDrive->SetExpiration(0.1);
        driveTrainRobotDrive->SetSensitivity(0.5);
        driveTrainRobotDrive->SetMaxOutput(1.0);
        driveTrainRobotDrive->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
        driveTrainRobotDrive->SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
	driveTrainQuadratureEncoderLeft = new Encoder(1, 1, 1, 2, false, Encoder::k4X);
	lw->AddSensor("Drive Train", "Quadrature Encoder Left", driveTrainQuadratureEncoderLeft);
	driveTrainQuadratureEncoderLeft->SetDistancePerPulse(1.0);
        driveTrainQuadratureEncoderLeft->SetPIDSourceParameter(Encoder::kRate);
        driveTrainQuadratureEncoderLeft->Start();
	driveTrainQuadratureEncoderRight = new Encoder(1, 3, 1, 4, false, Encoder::k4X);
	lw->AddSensor("Drive Train", "Quadrature Encoder Right", driveTrainQuadratureEncoderRight);
	driveTrainQuadratureEncoderRight->SetDistancePerPulse(1.0);
        driveTrainQuadratureEncoderRight->SetPIDSourceParameter(Encoder::kRate);
        driveTrainQuadratureEncoderRight->Start();
	shooterWheelPIDControllerShooterMotor = new Talon(1, 1);
	lw->AddActuator("Shooter Wheel PID Controller", "Shooter Motor", (Talon*) shooterWheelPIDControllerShooterMotor);
	
	shooterWheelPIDControllerShooterSpeedEncoder = new Encoder(1, 6, 2, 1, false, Encoder::k4X);
	lw->AddSensor("Shooter Wheel PID Controller", "Shooter Speed Encoder", shooterWheelPIDControllerShooterSpeedEncoder);
	shooterWheelPIDControllerShooterSpeedEncoder->SetDistancePerPulse(1.0);
        shooterWheelPIDControllerShooterSpeedEncoder->SetPIDSourceParameter(Encoder::kRate);
        shooterWheelPIDControllerShooterSpeedEncoder->Start();
	shooterShooterTiltMotor = new Talon(1, 4);
	lw->AddActuator("Shooter", "Shooter Tilt Motor", (Talon*) shooterShooterTiltMotor);
	
	shooterShooterTiltEncoder = new Encoder(1, 7, 1, 8, false, Encoder::k4X);
	lw->AddSensor("Shooter", "Shooter Tilt Encoder", shooterShooterTiltEncoder);
	shooterShooterTiltEncoder->SetDistancePerPulse(1.0);
        shooterShooterTiltEncoder->SetPIDSourceParameter(Encoder::kRate);
        shooterShooterTiltEncoder->Start();
	shooterShooterTiltHomeSensor = new GearTooth(1, 9, false);
	lw->AddSensor("Shooter", "Shooter Tilt Home Sensor", shooterShooterTiltHomeSensor);
	
	shooterLatchFrisbeeSolenoid = new Solenoid(1, 5);
	lw->AddActuator("Shooter", "Latch Frisbee Solenoid", shooterLatchFrisbeeSolenoid);
	
	shooterLoadFrisbeeSolenoid = new Solenoid(1, 3);
	lw->AddActuator("Shooter", "Load Frisbee Solenoid", shooterLoadFrisbeeSolenoid);
	
	shooterShootSolenoid = new Solenoid(1, 4);
	lw->AddActuator("Shooter", "Shoot Solenoid", shooterShootSolenoid);
	
	frisbeeStorageFrisbeeIndexerMotor = new Talon(1, 7);
	lw->AddActuator("Frisbee Storage", "Frisbee Indexer Motor", (Talon*) frisbeeStorageFrisbeeIndexerMotor);
	
	frisbeeStorageFrisbeeIndexerSpeedEncoder = new Encoder(1, 10, 1, 11, false, Encoder::k4X);
	lw->AddSensor("Frisbee Storage", "Frisbee Indexer Speed Encoder", frisbeeStorageFrisbeeIndexerSpeedEncoder);
	frisbeeStorageFrisbeeIndexerSpeedEncoder->SetDistancePerPulse(1.0);
        frisbeeStorageFrisbeeIndexerSpeedEncoder->SetPIDSourceParameter(Encoder::kRate);
        frisbeeStorageFrisbeeIndexerSpeedEncoder->Start();
	frisbeeStorageFrisbeeInputOpticalSensor = new DigitalInput(1, 12);
	lw->AddSensor("Frisbee Storage", "Frisbee Input Optical Sensor", frisbeeStorageFrisbeeInputOpticalSensor);
	
	frisbeeStorageFrisbeeIndexOpticalSensor = new DigitalInput(1, 13);
	lw->AddSensor("Frisbee Storage", "Frisbee Index Optical Sensor", frisbeeStorageFrisbeeIndexOpticalSensor);
	
	frisbeeStorageFrisbeeReadytoshootOpticalSensor = new DigitalInput(1, 14);
	lw->AddSensor("Frisbee Storage", "Frisbee Ready to shoot Optical Sensor", frisbeeStorageFrisbeeReadytoshootOpticalSensor);
	
	frisbeePickupIntakeandCenteringRollerMotors = new Talon(1, 8);
	lw->AddActuator("Frisbee Pickup", "Intake and Centering Roller Motors", (Talon*) frisbeePickupIntakeandCenteringRollerMotors);
	
	pickupArmPickupArmMotor = new Talon(1, 9);
	lw->AddActuator("Pickup Arm", "Pickup Arm Motor", (Talon*) pickupArmPickupArmMotor);
	
	pickupArmPickupArmEncoder = new Encoder(2, 2, 2, 3, false, Encoder::k4X);
	lw->AddSensor("Pickup Arm", "Pickup Arm Encoder", pickupArmPickupArmEncoder);
	pickupArmPickupArmEncoder->SetDistancePerPulse(1.0);
        pickupArmPickupArmEncoder->SetPIDSourceParameter(Encoder::kRate);
        pickupArmPickupArmEncoder->Start();
	pickupArmPickupArmPIDController = new PIDController(1.0, 0.0, 0.0,/* F: 0.0, */ pickupArmPickupArmEncoder, pickupArmPickupArmMotor, 0.02);
	lw->AddActuator("Pickup Arm", "Pickup Arm PID Controller", pickupArmPickupArmPIDController);
	pickupArmPickupArmPIDController->SetContinuous(false); pickupArmPickupArmPIDController->SetAbsoluteTolerance(0.2); 
        pickupArmPickupArmPIDController->SetOutputRange(-1.0, 1.0);
	pickupArmPickupArmHomeSensor = new GearTooth(2, 4, false);
	lw->AddSensor("Pickup Arm", "Pickup Arm Home Sensor", pickupArmPickupArmHomeSensor);
	
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
}