/**
 * Get the PWM value directly from the hardware.
 *
 * Read a raw value from a PWM channel.
 *
 * @param channel The PWM channel used for this Victor
 * @return Raw PWM control value.  Range: 0 - 255.
 */
UINT8 GetVictorRaw(UINT32 channel)
{
	Victor *victor = (Victor *) AllocatePWM(channel, CreateVictorStatic);
	if (victor)
		return victor->GetRaw();
	else
		return 0;
}

/**
 * Set the PWM value directly to the hardware.
 *
 * Write a raw value to a PWM channel.
 *
 * @param slot The slot the digital module is plugged into
 * @param channel The PWM channel used for this Victor
 * @param value Raw PWM value.  Range 0 - 255.
 */
void SetVictorRaw(UINT32 slot, UINT32 channel, UINT8 value)
{
	Victor *victor = (Victor *) AllocatePWM(slot, channel, CreateVictorStatic);
	if (victor) victor->SetRaw(value);
}

/**
 * Set the PWM value.
 *
 * The PWM value is set using a range of -1.0 to 1.0, appropriately
 * scaling the value for the FPGA.
 *
 * @param channel The PWM channel used for this Victor
 * @param speed The speed value between -1.0 and 1.0 to set.
 */
void SetVictorSpeed(UINT32 channel, float speed)
{
	Victor *victor = (Victor *) AllocatePWM(channel, CreateVictorStatic);
	if (victor) victor->Set(speed);
}

void RobotDemo::MecanumDrive(float x, float y, float rotation, float gyroAngle)
{
	const float EPSILONFL=1.f, EPSILONFR=1.f, EPSILONBL=1.f, EPSILONBR=1.f;
		
	double xIn = x;
	double yIn = y;
	// Negate y for the joystick.
	yIn = -yIn;
	// Compenstate for gyro angle.
	RotateVector(xIn, yIn, gyroAngle);

	double wheelSpeeds[kMaxNumberOfMotors];
	wheelSpeeds[0] = xIn + yIn + rotation;
	wheelSpeeds[1] = -xIn + yIn - rotation;
	wheelSpeeds[2] = -xIn + yIn + rotation;
	wheelSpeeds[3] = xIn + yIn - rotation;

	Normalize(wheelSpeeds);

	uint8_t syncGroup = 0x80;

	victorFL->Set(EPSILONFL*wheelSpeeds[0] * 1, syncGroup);
	victorFR->Set(EPSILONFR*wheelSpeeds[1] * -1, syncGroup);
	victorBL->Set(EPSILONBL*wheelSpeeds[2] * 1, syncGroup);
	victorBR->Set(EPSILONBR*wheelSpeeds[3] * -1, syncGroup);

	CANJaguar::UpdateSyncGroup(syncGroup);
}

	void TeleopPeriodic() {
		if(m_driver->GetRawButton(BUTTON_LB)) {
			// PYRAMID
			m_PIDController->SetSetpoint(PLATE_PYRAMID_THREE_POINT);
			m_PIDController->Enable();
		} else if(m_driver->GetRawButton(BUTTON_RB)) {
			// FEEDER
			m_PIDController->SetSetpoint(PLATE_FEEDER_THREE_POINT);
			m_PIDController->Enable();
		} else if(m_driver->GetRawAxis(TRIGGERS) > 0.5) {
			m_PIDController->SetSetpoint(PLATE_TEN_POINT_CLIMB);
			m_PIDController->Enable();
		} else {
			// MANUAL CONTROL
			m_PIDController->Disable();
			
			m_plate1->Set(-deadband(m_driver->GetRawAxis(LEFT_Y)));
			m_plate2->Set(-deadband(m_driver->GetRawAxis(LEFT_Y)));
		}
		
		// ----- PRINT -----
		SmartDashboard::PutNumber("Plate Position: ", m_plateSensor->GetVoltage());
		SmartDashboard::PutNumber("PID GET: ", m_plateSensor->PIDGet());
		
	} // TeleopPeriodic()

void RobotDemo::dumb_climber_code()
{
	//printf("Top:%i   Bottom:%i  " , top_claw_limit_switch->Get() , bottom_claw_limit_switch->Get());
	if (drive_stick_prim ->GetRawButton(climber_hold_up))
	{
		if (top_claw_limit_switch->Get() == 1)
		{
			climbing_motor->Set(0.0);
			//printf("STOPPED\n");
		}
		else
		{
			climbing_motor->Set(1);
			//printf("GOING UP\n");
		}
	} // not climber hold up

	else if (drive_stick_prim->GetRawButton(climber_hold_down))
	{
		if (bottom_claw_limit_switch->Get() == 0)
		{
			climbing_motor->Set(0.0);
			//printf("STOPED\n");
		}
		else
		{
			climbing_motor->Set(-1);
			//printf("GOING DOWN\n");
		}
	} // hold down button
	else
	{ // no js buttons pushed
		climbing_motor->Set(0.0);
	}
}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		while (IsOperatorControl())
		{
			left.Set(cont.GetRawAxis(2));
			right.Set(cont.GetRawAxis(4));
		}
	}

void RobotDemo::dumb_drive_code()
{
#if DUMB_DRIVE_CODE
	left_drive_motor_A->Set(-drive_stick_sec ->GetY());
	left_drive_motor_B->Set(-drive_stick_sec->GetY());
	right_drive_motor_A->Set(drive_stick_prim->GetY());
	right_drive_motor_B->Set(drive_stick_prim->GetY());
#endif
}

 void AutonomousPeriodic(void)
 {
 	// Basic motor test code 
 	leftFrontDrive->Set(1);
 	leftRearDrive->Set(1);
 	
 	rightRearDrive->Set(1);
 	rightFrontDrive->Set(1);
 }

	/**
	 * This method is called every 20ms to update the robots components during autonomous.
	 * There should be no blocking calls in this method (connection requests, large data collection, etc),
	 * failing to comply with this could result in inconsistent robot behavior
	 */
	void AutonomousPeriodic()
	{
		//In the first two seconds of auto, drive forwardat half power
		if(Timer::GetFPGATimestamp() - autoTimer >= 0 && Timer::GetFPGATimestamp() < 2)
		{
			rd->SetLeftRightMotorOutputs(.5, .5);
		}
		else if(Timer::GetFPGATimestamp() - autoTimer >= 2 && Timer::GetFPGATimestamp() < 4)
		{
			//2 to 4 seconds, stop drivetrain and spin shooter wheels
			rd->SetLeftRightMotorOutputs(0, 0);
			shoot1->Set(1);
			shoot2->Set(1);
		}
		else if(Timer::GetFPGATimestamp() - autoTimer >= 4 && Timer::GetFPGATimestamp() < 8 && !kickerSwitch->Get())
		{
			//4 to 8 seconds and while kicker switch is not true, spin shooter wheels and kicker
			shoot1->Set(1);
			shoot2->Set(1);
			kicker->Set(-1);
		}
		else
		{
			//After all that, stop everything
			rd->SetLeftRightMotorOutputs(0, 0);
			shoot1->Set(0);
			shoot2->Set(0);
			kicker->Set(0);
		}
	}

 void Reset() {
   m_talonCounter->Reset();
   m_victorCounter->Reset();
   m_jaguarCounter->Reset();
   m_talon->Set(0.0f);
   m_victor->Set(0.0f);
   m_jaguar->Set(0.0f);
 }

	/**
	 * This method is called every 20ms to update the robots components during teleop.
	 * There should be no blocking calls in this method (connection requests, large data collection, etc),
	 * failing to comply with this could result in inconsistent robot behavior
	 */
	void TeleopPeriodic()
	{
		//Make the robot move like an arcade stick controlled device
		rd->ArcadeDrive(j1);

		//If button 1 on the joystick is pressed, spin the shooter wheels up,
		//otherwise stop them
		if(j1->GetRawButton(1))
		{
			shoot1->Set(1);
			shoot2->Set(2);
		}
		else
		{
			shoot1->Set(0);
			shoot2->Set(0);
		}

		//If button 2 on the joystick is pressed, spin the kicker, otherwise stop the kicker
		if(j1->GetRawButton(2))
		{
			kicker->Set(-1);
		}
		else
		{
			kicker->Set(0);
		}
	}

	void Autonomous()
	{
		compressor.Start();//start compressor
		myRobot.SetSafetyEnabled(false);
		myRobot.Drive(-0.5, 0.0); 	// drive forwards half speed
		Wait(2.0); 				//    for 2 seconds
		myRobot.Drive(0.0, 0.0); 	// stop robot
		
		
		while (!shoot.Get()){//while shooter isnt cocked pull it back
			shooter.SetSpeed(1); //set motor
		}
		
		shooterSole.Set(shooterSole.kForward);//Sets Solenoid forward, shoot ball
		shooterSole.Set(shooterSole.kReverse);//Sets Solenoid backward
		
		myRobot.Drive(1.0, -1.0);//turn around for 0.5 seconds, we have to check to see if it takes that long
		Wait(0.5);//wait for 0.5 seconds
		myRobot.Drive(0.0, 0.0);//stop robot
	}

	void ShootModeSet()
	{
		if (controller.GetRawButton(BTN_MODE_RELOAD) == true && ShootMode == eShoot)
		{
			ShootMode = eReload;
			spinspeed = 0;
			shootertime.Stop();
			shootertime.Reset();
			shottime.Stop();
			shottime.Reset();
		}
		else if (controller.GetRawButton(BTN_MODE_SHOOT) == true && ShootMode == eReload)
		{
			ShootMode = eShoot;
			spinspeed = 1; // CHANGE BACK TO 1.  40% IS BUTTERZONE FOR CHILDREN
			shootertime.Start();
			shottime.Start();
		}
		spin1.Set(-spinspeed);
	}

	void Disabled(void)
	{
		bool stopped = false;
		while(!IsOperatorControl() && !IsAutonomous())
		{
			if(!stopped)
			{
				frontLeftMotor->Set(0.0);
				rearLeftMotor->Set(0.0);
				frontRightMotor->Set(0.0);
				rearRightMotor->Set(0.0);
				liftMotor1->Set(0.0);
				liftMotor2->Set(0.0);
				gripMotor1->Set(0.0);
				//gripMotor2->Set(0.0);
				PIDLift->Reset();
				PIDDriveLeft->Reset();
				PIDDriveRight->Reset();
				PIDGrip->Reset();
				stopped = true;
			}
		}
	}

	void TeleopPeriodic()
	{
		char myString[64];
		float   y;


// get the joystick position and filter the deadband
		y = -joystick->GetY();

		if (y > -0.1 && y < 0.1)
			y = 0;

		sprintf(myString, "joystick setting: %f\n", y);
		SmartDashboard::PutString("DB/String 1", myString);
		motor->Set(y, 0); // set the speed of the motor
		sprintf(myString, "gear count: %d\n", gearToothCounter->Get());
		SmartDashboard::PutString("DB/String 2", myString);
		sprintf(myString, "gear stopped: %d\n", gearToothCounter->GetStopped());
		SmartDashboard::PutString("DB/String 3", myString);

//		sprintf(myString, "In val: %d\n", toothInput->GetValue());
//		SmartDashboard::PutString("DB/String 2", myString);
	}

		void OperatorControl (void) {
			GetWatchdog().SetEnabled(true); // We do want Watchdog in Teleop, though.
			
			DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
			
			dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "                     ");
			dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode");
			
			SmartDashboard::GetInstance()->PutData("kinectMode?", kinectModeSelector);
			SmartDashboard::GetInstance()->PutData("demoMode?", demoModeSelector);
			SmartDashboard::GetInstance()->PutData("speedMode?", speedModeSelector);
			
			while (IsOperatorControl() && IsEnabled()) {
				GetWatchdog().Feed(); // Feed the Watchdog.
				
				kinectMode = (bool) kinectModeSelector->GetSelected();
				demoMode = (bool) demoModeSelector->GetSelected();
				speedModeMult = static_cast<double*>(speedModeSelector->GetSelected());
				
				if (kinectMode) {
					dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "                     ");
					dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Kinect Mode");
					
					if (!demoMode) {
						if (kinectDrive.GetRawButton(KINECT_FORWARD_BUTTON)) {
							motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult);
							motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult);
						} else if (kinectDrive.GetRawButton(KINECT_REVERSE_BUTTON)) {
							motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult);
							motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult);
						} else if (kinectDrive.GetRawButton(KINECT_LEFT_BUTTON)) {
							motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult);
							motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult);
						} else if (kinectDrive.GetRawButton(KINECT_RIGHT_BUTTON)) {
							motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult);
							motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult);
						} else {
							motorDriveLeft.Set(0);
							motorDriveRight.Set(0);
						}
					}
			
					if (kinectManipulator.GetRawButton(KINECT_SHOOT_BUTTON)) {
						motorShooter.Set(SHOOTER_MOTOR_POWER);
						motorFeed.Set(FEED_MOTOR_POWER);
						motorPickup.Set(PICKUP_MOTOR_POWER);
					} else if (kinectManipulator.GetRawButton(KINECT_SUCK_BUTTON)) {
						motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
						motorFeed.Set(FEED_MOTOR_POWER * -1);
						motorPickup.Set(PICKUP_MOTOR_POWER * -1);
					} else {
						motorShooter.Set(0);
						motorFeed.Set(0);
						motorPickup.Set(0);
					}
				
					if (kinectManipulator.GetRawButton(KINECT_TURRET_LEFT_BUTTON)) {
						motorTurret.Set(TURRET_POWER);
					} else if(kinectManipulator.GetRawButton(KINECT_TURRET_RIGHT_BUTTON)) {
						motorTurret.Set(TURRET_POWER * -1);
					} else {
						motorTurret.Set(0);
					}
				} else {
					if (joystickDriveLeft.GetThrottle() == 0) {
						dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "                     ");
						dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Xbox Mode");
						
						motorDriveLeft.Set(Deadband(joystickManipulator.GetRawAxis(2) * -1 * *speedModeMult));
						motorDriveRight.Set(Deadband(joystickManipulator.GetRawAxis(5) * *speedModeMult));
						
						if (joystickManipulator.GetRawButton(XBOX_SHOOT_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SHOOT_BUTTON))) {
							motorShooter.Set(SHOOTER_MOTOR_POWER);
							motorFeed.Set(FEED_MOTOR_POWER);
							motorPickup.Set(PICKUP_MOTOR_POWER);
						} else if (joystickManipulator.GetRawButton(XBOX_SUCK_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SUCK_BUTTON))) {
							motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
							motorFeed.Set(FEED_MOTOR_POWER * -1);
							motorPickup.Set(PICKUP_MOTOR_POWER * -1);
						} else {
							motorShooter.Set(0);
							motorFeed.Set(0);
							motorPickup.Set(0);
						}
						
						if (joystickManipulator.GetRawAxis(3) > 0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) > 0.2)) {
							motorTurret.Set(TURRET_POWER);
						} else if(joystickManipulator.GetRawAxis(3) < -0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) < -0.2)) {
							motorTurret.Set(TURRET_POWER * -1);
						} else {
							motorTurret.Set(0);
						}
					} else {
						dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "                     ");
						dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode");
						
						motorDriveLeft.Set(Deadband(joystickDriveLeft.GetY() * -1 * *speedModeMult));
						motorDriveRight.Set(Deadband(joystickDriveRight.GetY() * *speedModeMult));
						
						if (joystickManipulator.GetRawButton(MANIPULATOR_SHOOT_BUTTON)) {
//.........这里部分代码省略.........

	/********************************* Teleop methods *****************************************/
	void MotorControlLeft(float speed) 
	{
		left_1->SetSpeed(speed);
		left_2->SetSpeed(speed);
	}

	void MotorControlRight(float speed)
	{
		right_1->SetSpeed(speed);
		right_2->SetSpeed(speed);
	}

void RobotDemo::stop(){
	victorFL->Set(0.0);
	victorFR->Set(0.0);
	victorBL->Set(0.0);
	victorBR->Set(0.0);	
}