/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;
import com.grt192.mechanism.breakaway.GRTBreakawayRobotBase;
import com.grt192.sensor.*;
import com.grt192.actuator.*;
import com.grt192.controller.breakaway.auto.BreakAwayRobotPositioningController;
import com.grt192.controller.breakaway.teleop.BreakawayTeleopDriveController;
import com.grt192.controller.breakaway.teleop.KickerController;
import com.grt192.controller.breakaway.teleop.RecoveryController;
import com.grt192.controller.breakaway.teleop.RollerController;
import com.grt192.core.GRTRobot;
import com.grt192.mechanism.*;
import com.grt192.mechanism.breakaway.Kicker;
import com.grt192.mechanism.breakaway.Recovery;
import com.grt192.mechanism.breakaway.Roller;
/**
* The VM is configured to automatically run this class, and to call the
* functions corresponding to each mode, as described in the SimpleRobot
* documentation. If you change the name of this class or the package after
* creating this project, you must also update the manifest file in the resource
* directory.
*/
public class MainRobot extends GRTRobot {
//Global Controllers
//Autonomous Controllers
//Teleop Controllers
protected BreakawayTeleopDriveController driveControl;
protected KickerController kickerController;
protected RecoveryController recoveryController;
protected RollerController rollerController;
//Mechanisms
protected GRTDriverStation driverStation;
protected GRTBreakawayRobotBase robotbase;
protected Kicker shooter;
protected Roller rollers;
protected Recovery recovery;
public MainRobot() {
//Driver station components
GRTJoystick leftJoyStick = new GRTJoystick(1, 50, "left");
GRTJoystick rightJoyStick = new GRTJoystick(2, 50, "right");
GRTJoystick secondaryJoyStick = new GRTJoystick(3, 50, "secondary");
//digital outputs
GRTJaguar leftDT1 = new GRTJaguar(9);
GRTJaguar leftDT2 = new GRTJaguar(3);
GRTJaguar recovery1 = new GRTJaguar(5);
GRTJaguar recovery2 = new GRTJaguar(10);
GRTJaguar kicker = new GRTJaguar(1);
GRTJaguar roller1 = new GRTJaguar(4);
GRTJaguar roller2 = new GRTJaguar(2);
GRTJaguar rightDT1 = new GRTJaguar(6);
GRTJaguar rightDT2 = new GRTJaguar(8);
GRTServo kickerGate = new GRTServo(7);
//analog inputs
GRTAccelerometer ax1 = new GRTAccelerometer(2, 50, "BaseAccel");
GRTGyro gy1 = new GRTGyro(1, 15, "BaseGyro");
//digital inputs
GRTEncoder leftEn = new GRTEncoder(1, 2, 50, "LeftWheel");
GRTEncoder rightEn = new GRTEncoder(3, 4, 50, "RightWheel");
GRTSwitch kickerSwitch = new GRTSwitch(5, 50, "KickLimit");
GRTSwitch recoveryUpSwitch = new GRTSwitch(6, 50, "");
GRTSwitch recoveryGroundSwitch = new GRTSwitch(7, 50, "");
//Mechanisms
robotbase = new GRTBreakawayRobotBase(leftDT1, leftDT2, rightDT1, rightDT2,
ax1, gy1,
leftEn, rightEn);
driverStation = new GRTDriverStation(leftJoyStick,
rightJoyStick,
secondaryJoyStick);
shooter = new Kicker(kicker, kickerGate, kickerSwitch);
rollers = new Roller(roller1, roller2,
new GRTMaxBotixSonar(3, 50, "leftSonar"),
new GRTMaxBotixSonar(4, 50, "rightSonar"),
new GRTMaxBotixSonar(5, 50, "centerSonar"));
recovery = new Recovery(recovery1, recovery2,
recoveryUpSwitch, recoveryGroundSwitch,
new GRTAccelerometer(6, 50, ""));
//Controllers
driveControl = new BreakawayTeleopDriveController(robotbase, driverStation);
kickerController = new KickerController(driverStation, shooter);
rollerController = new RollerController(driverStation, rollers);
recoveryController = new RecoveryController(driverStation, recovery);
teleopControllers.addElement(driveControl);
teleopControllers.addElement(kickerController);
teleopControllers.addElement(recoveryController);
teleopControllers.addElement(rollerController);
this.getWatchdog().setEnabled(false);
System.out.println("Robot initialized OK");
}
}