Rahr

#include "WPILib.h"
#include "NetworkTables/NetworkTable.h"


class Robot: public IterativeRobot {

public:
	// Network Table Test
	std::shared_ptr<NetworkTable> table;

private:
	Joystick xbox;

	// Solenoid Control
	DoubleSolenoid *sol;

	CANTalon testMotor;
	int position;





public:
	Robot():
		xbox(0),

//		sol(1,2),
//		solVal(DoubleSolenoid::Value::kForward),

		testMotor(9),//Tilter==5
		position(0)

	{
		sol = new DoubleSolenoid(1,2);

		testMotor.SetFeedbackDevice(CANTalon::QuadEncoder);
		testMotor.SetControlMode(CANTalon::kPosition);
		testMotor.SetPID(0,0,0);

		table = NetworkTable::GetTable("Rio_to_Client");
	}

private:

	void RobotInit(){
		//No idea when this is called
	}

	void AutonomousInit(){
		//Called once at the start of each Auto period
	}

	void AutonomousPeriodic(){
		//Called PERIODICALLY during the Auto period
	}

	void TeleopInit(){
		//Called once at the start of each operator period
		printf("TELEOP Initialized\n");
	}
	void DisabledInit(){
		printf("Disabled TELEOP\n");
	}

	void TeleopPeriodic(){
		//Called PERIODICALLY during the operator period

		float valX = xbox.GetRawAxis(XBox::rightX);
//		float valY = xbox.GetRawAxis(XBox::rightY);

//		SmartDashboard::PutString("DB/String 0","Joystick: [ %4f , "+str(valY)+" ]");
//		printf("Joystick: [ %.4f , %.4f]\n",valX,valY);
//		printf("X: [ %.4f ]\tTalon: [ %.4f ]\n",valX,testMotor.GetOutputVoltage());

		// abs(x) function is funky. if (abs(valX) > 0.015) -> -1 or 0 or 1 talon output


//		SmartDashboard::PutString("DB/String 0","Encoder:");
//		SmartDashboard::PutString(testMotor.GetEncPosition());

		bool solMode = false;

		if (solMode){
			if (xbox.GetRawButton(XBox::b)){
	//			solVal = DoubleSolenoid::Value::kForward;
				sol->Set(DoubleSolenoid::Value::kForward);
				printf("Go Forward\n");
			} else if (xbox.GetRawButton(XBox::x)){
	//			solVal = DoubleSolenoid::Value::kReverse;
				sol->Set(DoubleSolenoid::Value::kReverse);
				printf("Go Back\n");
			} else {
	//			solVal = DoubleSolenoid::Value::kOff;
				sol->Set(DoubleSolenoid::Value::kOff);
				printf("Off\n");
			}
	//		printf("SolVal: %d\n",solVal);


	//		printf("Where it is: %d\n",testMotor.GetEncPosition());
	//		printf("Where it wants to be: %d\n",position);

			if (valX >0.15 || valX < -0.15){
				position += valX*10;
			}
			//Read in PID values from SD
			testMotor.SetP(SmartDashboard::GetNumber("DB/Slider 0",0));
			testMotor.SetI(SmartDashboard::GetNumber("DB/Slider 1",0));
			testMotor.SetD(SmartDashboard::GetNumber("DB/Slider 2",0));

			testMotor.Set(position);
	//		sol->Set(solVal);
		}//solMode

		/*
		{//Test1
		float x = table->GetNumber("X",4);
		printf("X: %f\n",x);

		table->PutNumber("VAL",valX);
		printf("Y:%f\n",table->GetNumber("VAL",-1));
		}
		*/

		table->PutNumber("Cheesy",254);
//		printf("ClientVal: %f\n",table->GetNumber("ClientVal",254));
	}

	void TestPeriodic(){
		//Who knows (We don't use this)
	}
};

START_ROBOT_CLASS(Robot)