Updated

src/IAMRobot.cpp

@@ -7,25 +7,21 @@
 
 #include <IAMRobot.h>
 using namespace frc;
-IAMRobot::IAMRobot() {
 
-	flTalon = new CANTalon(flTalID);
-	frTalon = new CANTalon(frTalID);
-	rlTalon = new CANTalon(rlTalID);
-	rrTalon = new CANTalon(rrTalID);
-	drive  = new RobotDrive(flTalon,rlTalon,frTalon,rrTalon);
+IAMRobot::IAMRobot() {
 	mainController = new XboxController(mainControllerID);
-	driveEncoder1 = new Encoder(driveEncoder1Port1,driveEncoder1Port2,true,Encoder::EncodingType::k4X);
-	driveEncoder2 = new Encoder(driveEncoder2Port1,driveEncoder2Port2,true,Encoder::EncodingType::k4X);
+	driveSystem = new DriveSystem(mainController);
+	pickup = new GearPickup(mainController);
+	baller = new Ball_Pickup(mainController);
+	lifter = new Lifter(mainController);
 }
 
 IAMRobot::~IAMRobot() {
-	delete drive;
 	delete mainController;
-	delete flTalon;
-	delete frTalon;
-	delete rlTalon;
-	delete rrTalon;
+	delete driveSystem;
+	delete pickup;
+	delete baller;
+	delete lifter;
 }
 
 void IAMRobot::RobotInit(){
@@ -41,9 +37,10 @@ void IAMRobot::TeleopInit(){
 
 }
 void IAMRobot::TeleopPeriodic(){
-	drive->ArcadeDrive(mainController->GetY(Joystick::kLeftHand),mainController->GetX(Joystick::kRightHand),false);
-	SmartDashboard::PutNumber("Encoder1 Get:", driveEncoder1->Get());
-	SmartDashboard::PutNumber("Encoder2 Get:", driveEncoder2->Get());
+	driveSystem->ArcadeDriveStickSquare();
+	pickup->teleopGearLoop();
+	baller->TeleopBallLoop();
+	lifter->TeleopLoop();
 }
 void IAMRobot::TestPeriodic(){
 

src/IAMRobot.h

@@ -14,7 +14,13 @@
 #include <string>
 
 #include <IterativeRobot.h>
+#include "systems/DriveSystem.h"
+#include "util/DeviceIDs.h"
+#include "systems/GearPickup.h"
+#include "systems/BallPickup.h"
+#include "systems/Lifter.h"
 using namespace frc;
+
 class IAMRobot: public IterativeRobot {
 
 public:
@@ -23,24 +29,10 @@ class IAMRobot: public IterativeRobot {
 
 private:
 	XboxController *mainController;
-	CANTalon *flTalon;
-	CANTalon *frTalon;
-	CANTalon *rlTalon;
-	CANTalon *rrTalon;
-	RobotDrive *drive;
-	Encoder* driveEncoder1;
-	Encoder* driveEncoder2;
-
-	int driveEncoder1Port1;
-	int driveEncoder1Port2;
-	int driveEncoder2Port1;
-	int driveEncoder2Port2;
-	int flTalID = 1;
-	int frTalID = 3;
-	int rlTalID = 2;
-	int rrTalID = 4;
-	int mainControllerID = 0;
-
+	DriveSystem* driveSystem;
+	GearPickup* pickup;
+	Ball_Pickup* baller;
+	Lifter* lifter;
 	void RobotInit() override;
 	void AutonomousInit() override;
 	void AutonomousPeriodic() override;

src/systems/BallPickup.cpp

@@ -0,0 +1,64 @@
+/*
+ * BallPickup.cpp
+ *
+ *  Created on: Feb 17, 2017
+ *      Author: Anish
+ */
+
+#include <systems/BallPickup.h>
+
+Ball_Pickup::Ball_Pickup(XboxController* controller) {
+	this->control = controller;
+	innerRoller = new CANTalon(innerRollerID);
+	outerRoller = new CANTalon(outerRollerID);
+	Ball_BackDoor = new DoubleSolenoid(backDoorID1,backDoorID2);
+}
+
+Ball_Pickup::~Ball_Pickup() {
+	delete control;
+	delete innerRoller;
+	delete outerRoller;
+	delete Ball_BackDoor;
+
+}
+
+void Ball_Pickup::TeleopBallLoop(){
+	if(control->GetXButton() && !control->GetYButton()){
+		BallInput();
+	}
+	else if(control->GetYButton() && !control->GetXButton()){
+		BallOutput();
+	}
+
+	else if(control->GetTriggerAxis(Joystick::kRightHand) && !control->GetTriggerAxis(Joystick::kLeftHand)){
+		RaiseElevator();
+	}
+	else if(control->GetTriggerAxis(Joystick::kRightHand) && !control->GetTriggerAxis(Joystick::kLeftHand)){
+		LowElevator();
+	}
+
+	else{
+		innerRoller->Set(0);
+		outerRoller->Set(0);
+	}
+}
+
+void Ball_Pickup::BallInput(){
+	innerRoller->Set(1);
+	outerRoller->Set(1);
+}
+
+void Ball_Pickup::BallOutput(){
+	innerRoller->Set(-1);
+	outerRoller->Set(-1);
+}
+
+void Ball_Pickup::RaiseElevator(){
+	Ball_BackDoor->Set(DoubleSolenoid::Value::kForward);
+}
+
+void Ball_Pickup::LowElevator(){
+	Ball_BackDoor->Set(DoubleSolenoid::Value::kReverse);
+}
+
+

src/systems/BallPickup.h

@@ -0,0 +1,31 @@
+/*
+ * BallPickup.h
+ *
+ *  Created on: Feb 17, 2017
+ *      Author: Anish
+ */
+
+#ifndef SRC_SYSTEMS_BALLPICKUP_H_
+#define SRC_SYSTEMS_BALLPICKUP_H_
+#include "WPILib.h"
+#include "CANTalon.h"
+#include "teleop/controls/Deadband.h"
+#include "util/DeviceIDs.h"
+#include "systems/Constants.h"
+using namespace frc;
+class Ball_Pickup {
+public:
+	Ball_Pickup(XboxController* controller);
+	virtual ~Ball_Pickup();
+	CANTalon* outerRoller;
+	CANTalon* innerRoller;
+	XboxController* control;
+	DoubleSolenoid* Ball_BackDoor;
+	void TeleopBallLoop();
+	void BallInput();
+	void BallOutput();
+	void RaiseElevator();
+	void LowElevator();
+};
+
+#endif /* SRC_SYSTEMS_BALLPICKUP_H_ */

src/systems/Constants.h

@@ -0,0 +1,9 @@
+#ifndef CONST
+#define CONST
+const double LowLimitDriveDeadband = 0.25;
+const double HighLimitDriveDeadband = -0.25;
+const double GearPickWaitTime = 0.5;
+const double SpeedScale = 0.75;
+const double RollerSpeed = 0.75;
+const double LiftSpeed = 0.75;
+#endif

src/systems/DriveSystem.cpp

@@ -5,14 +5,57 @@
  *      Author: Anish
  */
 
-#include <systems/DriveSystem.h>
 
-DriveSystem::DriveSystem() {
+#include "DriveSystem.h"
+using namespace frc;
 
 
+DriveSystem::DriveSystem(XboxController* mainController) {
+	flTalon = new CANTalon(flTalID);
+	frTalon = new CANTalon(frTalID);
+	rlTalon = new CANTalon(rlTalID);
+	rrTalon = new CANTalon(rrTalID);
+	drive  = new RobotDrive(flTalon,rlTalon,frTalon,rrTalon);
+	driveEncoder1 = new Encoder(flEncID1,flEncID2,true,Encoder::EncodingType::k4X);
+	driveEncoder2 = new Encoder(frEncID1,frEncID2,true,Encoder::EncodingType::k4X);
+	controller = mainController;
+	driveband = new Deadband(LowLimitDriveDeadband,HighLimitDriveDeadband);
 }
 
 DriveSystem::~DriveSystem() {
+	delete drive;
+	delete driveEncoder1;
+	delete driveEncoder2;
+	delete flTalon;
+	delete frTalon;
+	delete rlTalon;
+	delete rrTalon;
+	delete controller;
+	delete driveband;
+}
+
+
+
+void DriveSystem::ArcadeDriveStick(){
+		drive->ArcadeDrive(SpeedScale*driveband->ReturnBoundedValue(controller->GetY(Joystick::kLeftHand)), SpeedScale*driveband->ReturnBoundedValue(controller->GetX(Joystick::kRightHand)), false);
+
+}
+
+void DriveSystem::TankDriveStick(){
+	drive->TankDrive(SpeedScale*driveband->ReturnBoundedValue(controller->GetY(Joystick::kLeftHand)),SpeedScale*driveband->ReturnBoundedValue(controller->GetY(Joystick::kRightHand)),false);
+}
 
+void DriveSystem::ArcadeDriveStickSquare(){
+	drive->ArcadeDrive(SpeedScale*driveband->ReturnBoundedValue(controller->GetY(Joystick::kLeftHand)), SpeedScale*driveband->ReturnBoundedValue(controller->GetX(Joystick::kRightHand)), true);
+
+	if(controller->GetPOV()==0){
+		drive->ArcadeDrive(-1,0,true);
+	}
+	else if(controller->GetPOV()==180){
+		drive->ArcadeDrive(1,0,true);
+	}
+}
+void DriveSystem::TankDriveStickSquare(){
+	drive->TankDrive(driveband->ReturnBoundedValue(controller->GetY(Joystick::kLeftHand)),driveband->ReturnBoundedValue(controller->GetY(Joystick::kRightHand)),true);
 }
 

src/systems/DriveSystem.h

@@ -8,13 +8,33 @@
 #ifndef SRC_SYSTEMS_DRIVESYSTEM_H_
 #define SRC_SYSTEMS_DRIVESYSTEM_H_
 #include "WPILib.h"
+#include "CANTalon.h"
+#include "teleop/controls/Deadband.h"
+#include "util/DeviceIDs.h"
+#include "systems/Constants.h"
+using namespace frc;
+
 class DriveSystem {
 public:
-	DriveSystem();
 	virtual ~DriveSystem();
+	CANTalon *flTalon;
+	CANTalon *frTalon;
+	CANTalon *rlTalon;
+	CANTalon *rrTalon;
+	RobotDrive *drive;
+	Encoder* driveEncoder1;
+	Encoder* driveEncoder2;
+	XboxController* controller;
+	DriveSystem(XboxController* mainController);
 	void DriveArcade();
-
-private:
+	void AnishArcadeDrive();
+	void AnishArcadeDriveSquare();
+	void ArcadeDriveStick();
+	void ArcadeDriveStickSquare();
+	void TankDriveStick();
+	void TankDriveStickSquare();
+	Deadband* driveband;
+//private:
 
 
 };

src/systems/GearPickup.cpp

@@ -0,0 +1,93 @@
+/*
+ * GearPickup.cpp
+ *
+ *  Created on: Feb 11, 2017
+ *      Author: Anish
+ */
+
+#include <systems/GearPickup.h>
+
+GearPickup::GearPickup(XboxController*controller) {
+	this->controller = controller;
+	liftSolenoid = new DoubleSolenoid(PCM_ID, liftSolenoidID1,liftSolenoidID2);
+	clampSolenoid = new DoubleSolenoid(PCM_ID,clampSolenoidID1,clampSolenoidID2);
+	banner = new DigitalInput(BannerID);
+}
+
+GearPickup::~GearPickup() {
+	delete controller;
+	delete liftSolenoid;
+	delete clampSolenoid;
+	delete banner;
+}
+
+void GearPickup::teleopGearLoop(){
+	banner->UpdateTable();
+	SmartDashboard::PutBoolean("Banner", banner->Get());
+	if(isliftSolenoid && controller->GetBumper(Joystick::kRightHand)){
+		isliftSolenoid  = false;
+		liftSolenoid->Set(DoubleSolenoid::Value::kReverse);
+		Wait(GearPickWaitTime);
+
+	}
+	else if(!isliftSolenoid && controller->GetBumper(Joystick::kRightHand)){
+		isliftSolenoid  = true;
+		liftSolenoid->Set(DoubleSolenoid::Value::kForward);
+		Wait(GearPickWaitTime);
+	}
+	else if(isclampSolenoid && controller->GetBumper(Joystick::kLeftHand)){
+		isclampSolenoid = false;
+		clampSolenoid->Set(DoubleSolenoid::Value::kReverse);
+		Wait(GearPickWaitTime);
+	}
+	else if(!isclampSolenoid && controller->GetBumper(Joystick::kLeftHand)){
+		isclampSolenoid = true;
+		clampSolenoid->Set(DoubleSolenoid::Value::kForward);
+		Wait(GearPickWaitTime);
+	}
+
+}
+
+void GearPickup::teleopGearLoopMod(){
+	if(liftSolenoid->Get() == DoubleSolenoid::Value::kForward && controller->GetBumper(Joystick::kRightHand)){
+		liftSolenoid->Set(DoubleSolenoid::Value::kReverse);
+		Wait(GearPickWaitTime);
+	}
+	else if(liftSolenoid->Get() == DoubleSolenoid::Value::kReverse && controller->GetBumper(Joystick::kRightHand)){
+		liftSolenoid->Set(DoubleSolenoid::Value::kForward);
+		Wait(GearPickWaitTime);
+	}
+	else if (clampSolenoid->Get() == DoubleSolenoid::Value::kReverse && controller->GetBumper(Joystick::kLeftHand)){
+		clampSolenoid->Set(DoubleSolenoid::Value::kForward);
+		Wait(GearPickWaitTime);
+	}
+	else if(clampSolenoid->Get() == DoubleSolenoid::Value::kForward && controller->GetBumper(Joystick::kLeftHand)){
+		clampSolenoid->Set(DoubleSolenoid::Value::kReverse);
+		Wait(GearPickWaitTime);
+	}
+}
+
+void GearPickup::teleopGearLoopMod2(){
+	if(liftSolenoid->Get() == DoubleSolenoid::Value::kForward && controller->GetBumper(Joystick::kRightHand)){
+		while(liftSolenoid->Get() != DoubleSolenoid::Value::kReverse){
+			liftSolenoid->Set(DoubleSolenoid::Value::kReverse);
+		}
+	}
+	else if(liftSolenoid->Get() == DoubleSolenoid::Value::kReverse && controller->GetBumper(Joystick::kRightHand)){
+		while(liftSolenoid->Get() != DoubleSolenoid::Value::kForward){
+			liftSolenoid->Set(DoubleSolenoid::Value::kForward);
+		}
+	}
+	else if (clampSolenoid->Get() == DoubleSolenoid::Value::kReverse && controller->GetBumper(Joystick::kLeftHand)){
+		while(clampSolenoid->Get() != DoubleSolenoid::Value::kForward){
+			clampSolenoid->Set(DoubleSolenoid::Value::kForward);
+		}
+	}
+	else if(clampSolenoid->Get() == DoubleSolenoid::Value::kForward && controller->GetBumper(Joystick::kLeftHand)){
+		while(liftSolenoid->Get() != DoubleSolenoid::Value::kReverse){
+			liftSolenoid->Set(DoubleSolenoid::Value::kReverse);
+		}
+	}
+}
+
+