Ejemplo n.º 1
0
	void RobotInit(void) {
		// Actions which would be performed once (and only once) upon initialization of the
		// robot would be put here.
		
		// Test to see if Dashboard is connected---------------------------------------------------------
//		printf("HUE MIN: %d\n", SmartDashboard::GetNumber("HUE MIN"));
		
		// Initialize exponent value from SmartDashboard
		exp = 3.0;
		
		buttonWasDown = false;
		// Initialize settings for encoder drive PIDControllers
//		Drive_PID_Controller->SetOutputRange(-0.2, 0.2);
//		Drive_PID_Controller->SetTolerance(0.1);
		
		// Initialize settings for RobotTurn
//		Goal_Align_PID->SetOutputRange(-0.2, 0.2);
//		Goal_Align_PID->SetTolerance(0.1);
        
		// Set encoders
        Front_R->ConfigEncoderCodesPerRev(360);
        Front_L->ConfigEncoderCodesPerRev(360);
		
        // Set each drive motor to have an encoder to be its friend
        Front_R->SetPositionReference(CANJaguar::kPosRef_QuadEncoder);
        Front_L->SetPositionReference(CANJaguar::kPosRef_QuadEncoder);
        
		
		printf("RobotInit() completed.\n");
	}
Ejemplo n.º 2
0
	void Disabled (void) {
		debug("disable");			
		Dlf->StopMotor();
		Dlb->StopMotor();
		Drf->StopMotor();
		Drb->StopMotor();
	}
Ejemplo n.º 3
0
	void Drive (float speed, float turn, float strafe) {
		Dlf->Set(range(speed + turn + strafe)*250, 2);
		Dlb->Set(range(speed + turn - strafe)*250, 2);
		Drf->Set(range(-speed + turn + strafe)*250, 2);
		Drb->Set(range(-speed + turn - strafe)*250, 2);
		//CANJaguar::UpdateSyncGroup(2);
					
	}
Ejemplo n.º 4
0
	void DoctaEight::leftyrighty(double left, double right)//set drive motors on either side
	{
		REDRUM;
		righty.Set(negate*right*.9);
		rightyB.Set(negate*right*.9);
		lefty.Set(negate*left*.9);
		leftyB.Set(negate*left*.9);
	}
Ejemplo n.º 5
0
	void setMotors(float left,float right)
	{
		left=-left;
		right=-right;
		left*=300;
		right*=300;
		frontLeft->Set(-left);
		frontRight->Set(right);
		backRight->Set(right);
		backLeft->Set(-left);
		//CANJaguar::UpdateSyncGroup(2);
	}
Ejemplo n.º 6
0
	CANRobotDemo()
		: speedJag(2, CANJaguar::kSpeed)
		, stick(1)
		, axis(Joystick::kXAxis)
		, commandTask("DashboardCommandServer", (FUNCPTR)DashboardCommandServerStub)
	{
		GetWatchdog().SetExpiration(100);
		speedJag.ConfigEncoderCodesPerRev(360);
		speedJag.ConfigMaxOutputVoltage(6.0);
		speedJag.ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		wpi_stackTraceEnable(true);
		commandTask.Start((INT32)this);
	}
Ejemplo n.º 7
0
	void UpdateDashboardStatus()
	{
		Dashboard &dashHigh = m_ds->GetHighPriorityDashboardPacker();
		dashHigh.AddCluster(); // PID (not used for now)
		dashHigh.AddDouble(0.0); // P
		dashHigh.AddDouble(0.0); // I
		dashHigh.AddDouble(0.0); // D
		dashHigh.FinalizeCluster();
		dashHigh.AddDouble(speedJag.GetSpeed()); // Current position
		dashHigh.AddDouble(speedJag.Get()); // Setpoint
		dashHigh.AddDouble(speedJag.GetOutputVoltage()); // Output Voltage
		dashHigh.Finalize();
	}
Ejemplo n.º 8
0
	void OperatorControl(void) {
		while(!IsDisabled()) {
			GetWatchdog().Feed();
			float speed = stick.GetRawAxis(2);
			float strafe = -1*stick.GetRawAxis(1);
			float turn = -1*stick.GetRawAxis(3);
			Dlf->Set(speed + turn + strafe);
			Dlb->Set(speed + turn - strafe);
			Drf->Set(-speed + turn + strafe);
			Drb->Set(-speed + turn - strafe);
			Wait(.05);	
		}
	}
Ejemplo n.º 9
0
	void TeleopInit(void) {
		m_telePeriodicLoops = 0;				// Reset the loop counter for teleop mode
		m_dsPacketsReceivedInCurrentSecond = 0;	// Reset the number of dsPackets in current second
		
		// Default autoPilot to off
		autoPilot = false;
		
		Front_R->EnableControl();
		Front_L->EnableControl();
		
		// Enable Goal Align PID
//		Goal_Align_PID->Disable(); // Stop previous enables
//		Goal_Align_PID->Enable();
//		Goal_Align_PID->SetSetpoint(0.0);
	}
Ejemplo n.º 10
0
	void RawControl::armAngle(float angle) {
		
		//angle/=300.0f;
		
		arm->Set(angle);
		//cout<<"angle= "<<angle<<"\n";
	}
Ejemplo n.º 11
0
 void AutonomousInit() {
     init();
     currentDistance = 0;
     goalDistance = 6.0*12.0; // 6 feet
     autonomousShooterDelay = 0;
     shooterMotor->Set(0);
     loaderMotor->Set(Relay::kOff);
 }
Ejemplo n.º 12
0
	void TeleopPeriodic(void) {
		// increment the number of teleop periodic loops completed
		m_telePeriodicLoops++;
		GetWatchdog().Feed();

//		if(autoPilot == true)
//		{
			// Auto Align Disable Button
//			if(operatorGamepad->GetButton(Joystick::kTopButton) == 2)
//			{
//				Goal_Align_PID->Disable(); // Stop outputs
//				Goal_Align_PID->Enable(); // Start PIDContoller up again
//				Goal_Align_PID->SetSetpoint(0.0);
//				autoPilot = false;
//			}
//		}
//		else
//		{
			// Calculate jaguar output based on exponent we pass from SmartDashboard
			double leftOutput, rightOutput;
			leftOutput = calculateDriveOutputForTeleop(operatorGamepad->GetRawAxis(2));
			rightOutput = calculateDriveOutputForTeleop(operatorGamepad->GetRawAxis(5));
			m_robotDrive->SetLeftRightMotorOutputs(leftOutput, rightOutput);
			
			if(operatorGamepad->GetRawButton(1) && !buttonWasDown)
			{
				printf("LEFT ENCODER: %f\n", Front_L->GetPosition());
				printf("RIGHT ENCODER: %f\n", Front_R->GetPosition());
			}
			
			buttonWasDown = operatorGamepad->GetRawButton(1);
			
			// Auto Align Button
//			if(operatorGamepad->GetButton(Joystick::kTopButton) == 1)
//			{
//				// Turn Auto Align on if we see a goal and we know the azimuth
//				if(SmartDashboard::GetBoolean(FOUND_KEY) == true)
//				{
//					Goal_Align_PID->SetSetpoint(SmartDashboard::GetNumber(AZIMUTH_KEY));
//					autoPilot = true;
//				}
//			}
//		}
		
	} 
Ejemplo n.º 13
0
    void StartWheels()
    {
	if (!spinFastNow) {
printf(">>> StartWheels\n");
	    Log(LOG_START, 0, 0);

	    spinFastNow = true;

	    // start shooter wheels in %vbus mode, max output
#ifdef HAVE_TOP_WHEEL
#ifdef HAVE_TOP_CAN1
	    jagVbus(topWheel1,    maxOutput);
	    Log(LOG_MODE, 1, 1);
#endif
#ifdef HAVE_TOP_PWM1
	    topWheel1->Set(maxOutput);
	    Log(LOG_MODE, 1, 1);
#endif
#ifdef HAVE_TOP_CAN2
	    jagVbus(topWheel2,    maxOutput);
	    Log(LOG_MODE, 2, 1);
#endif
#endif
#ifdef HAVE_BOTTOM_WHEEL
#ifdef HAVE_BOTTOM_CAN1
	    jagVbus(bottomWheel1, maxOutput);
	    Log(LOG_MODE, 3, 1);
#endif
#ifdef HAVE_BOTTOM_PWM1
	    bottomWheel1->Set(maxOutput);
	    Log(LOG_MODE, 3, 1);
#endif
#ifdef HAVE_BOTTOM_CAN2
	    jagVbus(bottomWheel2, maxOutput);
	    Log(LOG_MODE, 4, 1);
#endif
#endif
	    topPID = bottomPID = false;

	    // reset reporting counter
	    report = 0;
printf("<<< StartWheels\n");
	}
    }
Ejemplo n.º 14
0
    /**
     * Initialization code for test mode should go here.
     * 
     * Use this method for initialization code which will be called each time
     * the robot enters test mode.
     */
    void TestInit()
    {
printf(">>> TestInit\n");
#ifdef HAVE_COMPRESSOR
	compressor->Start();
#endif
#ifdef HAVE_ARM
	arm->Set(DoubleSolenoid::kOff);
#endif
#ifdef HAVE_INJECTOR
	injectorL->Set(DoubleSolenoid::kOff);
	injectorR->Set(DoubleSolenoid::kOff);
#endif
#ifdef HAVE_EJECTOR
	ejector->Set(false);
#endif
#ifdef HAVE_LEGS
	legs->Set(false);
#endif

#ifdef HAVE_TOP_WHEEL
#ifdef HAVE_TOP_CAN1
	jagVbus(topWheel1, 0.0);
#endif
#ifdef HAVE_TOP_PWM1
	topWheel1->Set(0.0);
#endif
#ifdef HAVE_TOP_CAN2
	jagVbus(topWheel2, 0.0);
#endif
#endif
#ifdef HAVE_BOTTOM_WHEEL
#ifdef HAVE_BOTTOM_CAN1
	jagVbus(bottomWheel1, 0.0);
#endif
#ifdef HAVE_BOTTOM_PWM1
	bottomWheel1->Set(0.0);
#endif
#ifdef HAVE_BOTTOM_CAN2
	jagVbus(bottomWheel2, 0.0);
#endif
#endif
printf("<<< TestInit\n");
    }
Ejemplo n.º 15
0
	void TeleopPeriodic()
	{
		SmartDashboard::PutNumber("joystickX",stick.GetX());
		SmartDashboard::PutNumber("joystickY",stick.GetY());
		//SmartDashboard::PutBoolean("f*****g buttons", stick.GetRawButton(1));

		//SmartDashboard::PutNumber("potentiometer voltage", pot.GetVoltage());
		SmartDashboard::PutBoolean("infra",infra.Get());

		SmartDashboard::PutNumber("accelX",accel.GetX());
		SmartDashboard::PutNumber("accelY",accel.GetY());
		SmartDashboard::PutNumber("accelZ",accel.GetZ());

		servo.Set(
			trueMap(stick.GetX(), 1, -1, 1, 0) // trueMap allows use of entire joystick
		);
		SmartDashboard::PutNumber("servo", servo.Get());


		jag1.Set(stick.GetY());
		jag2.Set(stick.GetY());
		//tal1.Set(stick.GetY());

		SmartDashboard::PutNumber("jag1", jag1.Get());
		SmartDashboard::PutNumber("jag2", jag2.Get());


		/*SmartDashboard::PutNumber("encpos", enc.Get());
		SmartDashboard::PutNumber("encspd", enc.GetRate());*/

		if (stick.GetRawButton(1) && !actuatePressed) {
			pistonVal=!pistonVal;
			piston.Set(pistonVal ? DoubleSolenoid::kForward : DoubleSolenoid::kReverse);

			actuatePressed = true;
		}
		else if (!stick.GetRawButton(1))
			actuatePressed = false;

		SmartDashboard::PutBoolean("piston forward", piston.Get() == DoubleSolenoid::kForward);


	}
Ejemplo n.º 16
0
	void Claw(double joy) {
		if(joy < 10) joy = 10;
		if(joy > 230) joy = 230;
		int location = EncClaw.Get();
		double speed = PIDClaw(joy, location);
		//if(location < 15) speed *= .2;
		if(speed > .32) speed = .32;
		if(speed < -.32) speed = -.32;
		if(speed < .1 && speed > -.1) speed = 0;
		arm2->Set(speed,2);
	}
Ejemplo n.º 17
0
    void StopWheels()
    {
	if (spinFastNow) {
printf(">>> StopWheels\n");
	    Log(LOG_STOP, 0, 0);

	    spinFastNow = false;

#ifdef HAVE_TOP_WHEEL
#ifdef HAVE_TOP_CAN1
	    jagStop(topWheel1);
	    Log(LOG_MODE, 1, 0);
#endif
#ifdef HAVE_TOP_PWM1
	    topWheel1->Disable();
	    Log(LOG_MODE, 1, 0);
#endif
#ifdef HAVE_TOP_CAN2
	    jagStop(topWheel2);
	    Log(LOG_MODE, 2, 0);
#endif
#endif
#ifdef HAVE_BOTTOM_WHEEL
#ifdef HAVE_BOTTOM_CAN1
	    jagStop(bottomWheel1);
	    Log(LOG_MODE, 3, 0);
#endif
#ifdef HAVE_BOTTOM_PWM1
	    bottomWheel1->Disable();
	    Log(LOG_MODE, 3, 0);
#endif
#ifdef HAVE_BOTTOM_CAN2
	    jagStop(bottomWheel2);
	    Log(LOG_MODE, 4, 0);
#endif
#endif

	    topPID = bottomPID = false;
printf("<<< StopWheels\n");
	}
    }
Ejemplo n.º 18
0
	/**
	 * Run the closed loop position controller on the Jag.
	 */
	void OperatorControl()
	{
		printf("In OperatorControl\n");
		GetWatchdog().SetEnabled(true);
		while (IsOperatorControl() && !IsDisabled())
		{
			GetWatchdog().Feed();
			// Set the desired setpoint
			speedJag.Set(stick.GetAxis(axis) * 150.0);
			UpdateDashboardStatus();
			Wait(0.05);
		}
	}
void MecanumDrive::InitMotor( CANJaguar& motor )
{
	motor.ChangeControlMode( m_currControlMode );
	if ( m_currControlMode == CANJaguar::kSpeed )
	{
		motor.ConfigEncoderCodesPerRev(360);
		motor.ConfigMaxOutputVoltage(12.0);
		motor.ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		motor.SetPID(.7,.004,0);
		motor.SetSpeedReference(CANJaguar::kSpeedRef_QuadEncoder);
	}
	motor.EnableControl();
}
void MecanumDrive::CheckForRestartedMotor( CANJaguar& motor, const char * strDescription )
{
        if ( m_currControlMode != CANJaguar::kSpeed )   // kSpeed is the default
        {
                if ( motor.GetPowerCycled() )
                {
                        Wait(0.10); // Wait 100 ms 
                        InitMotor( motor );
                        char error[256];
                        sprintf(error, "\n\n>>>>%s %s", strDescription, "Jaguar Power Cycled - re-initializing");
                        printf(error);
                        setErrorData(error, strlen(error), 100);                
                }
        }
}
Ejemplo n.º 21
0
	void Arm(double joy) {
		int location = EncArm.Get();
		/*
		if(location < 10 && joy < 0) joy = 0;
		if(location > 110 && joy > 0) joy = 0;
		arm1->Set(joy);
		arm1_sec->Set(joy);
		return;
		*/
		
		if(joy < -10) joy = -10;
		if(joy > 110) joy = 110;
		
		double speed = PIDArm(joy, location);
		if(speed > .5) speed = .5;
		if(speed < -.3) speed = -.3;
		if(speed < 0 && location < 10) speed = 0;
		if(speed > 0 && location > 110) speed = 0;
		speed = LowArm(speed);
		if(speed < .01 && speed > -.01) speed = 0;
		arm1->Set(speed,3);
		arm1_sec->Set(speed,3);
		
	}
Ejemplo n.º 22
0
	void RawControl::resetArm()
	{
		arm->SetPID(-5, 0, 0);
		arm->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		arm->SetSafetyEnabled(false);
		arm->SetPositionReference(CANJaguar::kPosRef_Potentiometer);
		arm->ConfigEncoderCodesPerRev(1);
		arm->EnableControl(0);
		
	}
Ejemplo n.º 23
0
    void AutonomousPeriodic() { 
    	
    	currentDistance = leftDriveEncoder->GetDistance();
		shooterMotor->Set(-shooterMotorVolts);
		
    	if (currentDistance >= goalDistance){
    		drive->setLeft(0);
    		drive->setRight(0);
    		loaderMotor->Set(Relay::kForward);
    	} else {
    		drive->setLeft(.49);
    		drive->setRight(.5);
    	}
        //log->info("LRD %f %f",
        //		leftDriveEncoder->GetDistance(),
        //		rightDriveEncoder->GetDistance());
    	//log->print();
    }
Ejemplo n.º 24
0
	void output (void)
	{
		REDRUM;
		if (IsAutonomous())
			driverOut->PrintfLine(DriverStationLCD::kUser_Line1, "blaarag");
		else if (IsOperatorControl())
		{	
			REDRUM;
		}
		outputCounter++;
					
					if (outputCounter % 30 == 0){
						REDRUM;
					driverOut->PrintfLine(DriverStationLCD::kUser_Line2, "Top Shooter RPM: %f",(float)LTop.GetSpeed());
					driverOut->PrintfLine(DriverStationLCD::kUser_Line3, "Bot Shooter RPM: %f",(float)LBot.GetSpeed());
			//		driverOut->PrintfLine(DriverStationLCD::kUser_Line6, "Pilot Z-Axis: %f",pilot.GetZ());
					
					}
					
					if (outputCounter % 60 == 0){
						REDRUM;
					driverOut->PrintfLine(DriverStationLCD::kUser_Line4, "Top CANJag Temp: %f Celcius",LTop.GetTemperature()*(9/5) + 32);
					driverOut->PrintfLine(DriverStationLCD::kUser_Line5, "Bot CANJag Temp: %f Celcius",LBot.GetTemperature()*(9/5) + 32);
				    outputCounter = 1;
					}
		driverOut->UpdateLCD();
	}//nom nom nom
Ejemplo n.º 25
0
	void RawControl::resetJags()
	{
		{
			frontRight->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
			frontRight->SetSafetyEnabled(false);
			frontRight->ConfigEncoderCodesPerRev(360);

			frontRight->EnableControl(0);

		}

		{
			frontLeft->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
			frontLeft->SetSafetyEnabled(false);
			frontLeft->ConfigEncoderCodesPerRev(360);
			frontLeft->EnableControl(0);

		}

		{
			backLeft->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
			backLeft->SetSafetyEnabled(false);
			backLeft->ConfigEncoderCodesPerRev(360);
			backLeft->EnableControl(0);

		}

		{
			backRight->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
			backRight->SetSafetyEnabled(false);
			backRight->ConfigEncoderCodesPerRev(360);
			backRight->EnableControl(0);

		}
		frontLeft->ChangeControlMode(CANJaguar::kPercentVbus);
		frontRight->ChangeControlMode(CANJaguar::kPercentVbus);
		backLeft->ChangeControlMode(CANJaguar::kPercentVbus);
		backRight->ChangeControlMode(CANJaguar::kPercentVbus);
		frontLeft->ConfigFaultTime(0);
		backLeft->ConfigFaultTime(0);
		backRight->ConfigFaultTime(0);
		frontRight->ConfigFaultTime(0);

		frontLeft->EnableControl(0);
		frontRight->EnableControl(0);
		backLeft->EnableControl(0);
		backRight->EnableControl(0);

	}
Ejemplo n.º 26
0
	void DoctaEight::OperatorControl(void)
	{
		REDRUM;
		LTop.SetSafetyEnabled(0);
		LTop.EnableControl();
		LBot.SetSafetyEnabled(0);
		LBot.EnableControl();
		while (IsOperatorControl())
		{
			REDRUM;
			output();
			if (pilot.GetRawButton(6))
			{
				arm.Set (.3);
			}
			
			else if (pilot.GetRawButton(5))
			{
				
				arm.Set (-.3);
			}
			else
				arm.Set (0);
				
		
			//Set Shooter state and reset RPMoffset if necessary
			
			if (copilot.GetRawButton(1)) //BUTTON A
			{	
				REDRUM;
				if(ShooterState != 1) 
				{
					REDRUM;
					ShooterState = 1;	
					RPMoffset = 0;
				}
			}
			else if (copilot.GetRawButton(2)) //BUTTON B
			{
				REDRUM;
				if(ShooterState != 2) 
				{
					REDRUM;
					ShooterState = 2;	
					RPMoffset = 0;
				}
			}
			else if (copilot.GetRawButton(3)) //BUTTON X
			{
				REDRUM;
				if(ShooterState != 3) 
				{
					REDRUM;
					ShooterState = 3;	
					RPMoffset = 0;
				}
			}
			else if (copilot.GetRawButton(4)) //BUTTON Y
			{
				REDRUM;
				if(ShooterState != 4) 
					{
						REDRUM;
						ShooterState = 4;	
						RPMoffset = 0;
					}
			}
			
			//increment or decrement RPMoffset
			if(copilot.GetRawButton(5)) //BUTTON LB
			{
				REDRUM;
				FlagB5 = true;
			}
			else if(copilot.GetRawButton(6)) //BUTTON RB
			{
				REDRUM;
				FlagB6 = true;
			}
			
			if(FlagB5 == true && copilot.GetRawButton(5) == false)
			{
				REDRUM;
				RPMoffset -= 50;
				FlagB5 = false;
			}
			else if(FlagB6 && !copilot.GetRawButton(6))
			{
				REDRUM;
				RPMoffset += 50;
				FlagB6 = false;
			}
			
			if (pilot.GetRawButton(1) && !cycle)
			{
				cycle = 1;
				negate*=-1;
			}
			else
			{
				cycle=0;
			}
			
			//prepare shooter/launcher ouput speed
			if(ShooterState == 1) { 
				REDRUM;		//BUTTON A
				LTopSpeed = MAX/12;
				LBotSpeed = RPMMid + RPMoffset;
			}
			else if(ShooterState == 2) { //BUTTON B
				REDRUM;
				LTopSpeed = MAX/12;
				LBotSpeed = RPMLow + RPMoffset;
			}
			else if(ShooterState == 3) { //BUTTON X
				REDRUM;
				LTopSpeed = MAX/12;
				LBotSpeed = RPMHigh + RPMoffset;
			}
			else if(ShooterState == 4) { //BUTTON Y
				REDRUM;
				LTopSpeed = 0;
				LBotSpeed = 0;
			}
			
			//set shooter launcher speed to CANJags                                                  
			LTop.Set(LTopSpeed);
			LBot.Set(-LBotSpeed);
			
			
			//move simple platform arm	
			leftyrighty(-pilot.GetY(), -pilot.GetRawAxis(4));
			
			
				
			
			intake.Set(-copilot.GetY());
			
			
		}
		//stops encoders
	}
Ejemplo n.º 27
0
	float setSpeed(float newMagnitude)
	{
		tarTheta = atan2(yVector, xVector);
		curTheta = -(posEncoder->GetVoltage() - FLOFFSET ) / 5 * 2 * PI;
		
		
		//	Code Snippet
		diffTheta = tarTheta - curTheta;
			
		if (diffTheta > PI) 
		{
			diffTheta -= 2*PI;
		} 
		else if (diffTheta < -PI) 
		{
			diffTheta += 2*PI;
		}

		if (diffTheta > PI/2) 
		{
			diffTheta -= PI;
			mag = mag * -1;
		} 
		else if (diffTheta < -PI/2) 
		{
			diffTheta += PI;
			mag = mag * -1;
		}

		turnVel = diffTheta / (PI/2);
		
		if (0 < turnVel && turnVel < .25)
		{
			turnVel = .25;
		} 
		if (0 > turnVel && turnVel > -.25)
		{
			turnVel = -.25;
		}
		if (fabs(diffTheta) < PI/45 )
		{
			turnVel = 0;
		}
		if (((turnVel > 0 && prevTurnVel < 0)
				|| (turnVel < 0&& prevTurnVel> 0)) 
				&& !changeSign)
		{
			changeSign = true;
//			moveTime = baneTimer.Get() + .1; 				**FIX BANETIMER
		}
		if (changeSign) 
		{
			turnVel = 0;
//			if (moveTime < baneTimer.Get()) 
			{
				changeSign = false;
			}
		}
		
		//	/Code Snippet
		
		
		if (!(xVector == 0 && yVector == 0))
		{
			turnWheel->Set(turnVel);							
			moveWheel->Set(mag);
		}
		else
		{
			turnWheel->Set(0);
			moveWheel->Set(0);
		}
		
		
	}
Ejemplo n.º 28
0
//CHECK THIS OUT!!
	void RawControl::configJags() {
		//will need to be tuned on the new robot
		frontLeft->SetPID(3, .07, 0);//tested values are 1,.02,0
		frontRight->SetPID(3, .09, 0);
		backLeft->SetPID(1, .013, 0);
		backRight->SetPID(1.2, .013, 0);
		arm->SetPID(ARM_P, -.02, 0);

		backLeft->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		frontLeft->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		frontRight->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		backRight->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		arm->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		//shoot everything remotely safety related
		backLeft->SetSafetyEnabled(false);
		backRight->SetSafetyEnabled(false);
		frontLeft->SetSafetyEnabled(false);
		frontRight->SetSafetyEnabled(false);
		arm->SetSafetyEnabled(false);
		
		
		

		frontLeft->ConfigMaxOutputVoltage(13);
		frontRight->ConfigMaxOutputVoltage(13);
		backLeft->ConfigMaxOutputVoltage(13);
		backRight->ConfigMaxOutputVoltage(13);
		arm->ConfigMaxOutputVoltage(13);

		frontLeft->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
		frontRight->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
		backRight->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
		backLeft->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
		arm->SetPositionReference(CANJaguar::kPosRef_Potentiometer);
		//not sure on these values either
		frontLeft->ConfigEncoderCodesPerRev(360);
		frontRight->ConfigEncoderCodesPerRev(360);
		backLeft->ConfigEncoderCodesPerRev(360);
		backRight->ConfigEncoderCodesPerRev(360);
		arm->ConfigPotentiometerTurns(1);
		frontLeft->ChangeControlMode(CANJaguar::kPercentVbus);
		frontRight->ChangeControlMode(CANJaguar::kPercentVbus);
		backLeft->ChangeControlMode(CANJaguar::kPercentVbus);
		backRight->ChangeControlMode(CANJaguar::kPercentVbus);
		frontLeft->EnableControl(0);
		frontRight->EnableControl(0);
		backLeft->EnableControl(0);
		backRight->EnableControl(0);
		frontLeft->ConfigFaultTime(0);
		backLeft->ConfigFaultTime(0);
		backRight->ConfigFaultTime(0);
		frontRight->ConfigFaultTime(0);
		arm->ConfigFaultTime(0);
		arm->EnableControl(0);
		arm->EnableControl(0);
		
		
		/*
		 fl=new CANJaguar(2,CANJaguar::kSpeed);
		 fr=new CANJaguar(3,CANJaguar::kSpeed);
		 bl=new CANJaguar(4,CANJaguar::kSpeed);
		 br=new CANJaguar(1,CANJaguar::kSpeed);
		 
		 fl->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
		 fr->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
		 br->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
		 bl->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
		 
		 fl->ConfigEncoderCodesPerRev(1440);
		 fr->ConfigEncoderCodesPerRev(1440);
		 bl->ConfigEncoderCodesPerRev(1440);
		 br->ConfigEncoderCodesPerRev(1440);
		 */
	}
Ejemplo n.º 29
0
	int RawControl::zero() {
		arm->EnableControl(0);
		
		return 0;
	}
Ejemplo n.º 30
0
	float RawControl::getArmPos()
	{
		return arm->GetPosition();
	}