void OperatorControl(void)
	{
		autonomous = false;
		//myRobot.SetSafetyEnabled(false);
		//myRobot.SetInvertedMotor(kFrontLeftMotor, true);
		//	myRobot.SetInvertedMotor(3, true);
		//variables for great pid
		double rightSpeed,leftSpeed;
		float rightSP, leftSP, liftSP, lastLiftSP, gripSP, tempLeftSP, tempRightSP;
		float stickY[2];
		float stickYAbs[2];
		bool lightOn = false;
		AxisCamera &camera = AxisCamera::GetInstance();
		camera.WriteResolution(AxisCameraParams::kResolution_160x120);
		camera.WriteMaxFPS(5);
		camera.WriteBrightness(50);
		camera.WriteRotation(AxisCameraParams::kRotation_0);
		rightEncoder->Start();
		leftEncoder->Start();
		liftEncoder->Start();
		rightEncoder->Reset();
		leftEncoder->Reset();
		liftEncoder->Reset();
		bool fastest = false; //transmission mode
		float reduction = 1; //gear reduction from 
		bool bDrivePID = false;
		//float maxSpeed = 500;
		float liftPower = 0;
		float liftPos = -10;
		bool disengageBrake = false;
		int count = 0;
		//int popCount = 0;
		double popStart = 0;
		double popTime = 0;
		int popStage = 0;
		int liftCount = 0;
		int liftCount2 = 0;
		const float LOG17 = log(17.61093344);
		float liftPowerAbs = 0;
		float gripError = 0;
		float gripErrorAbs = 0;
		float gripPropMod = 0;
		float gripIntMod = 0;
		bool shiftHigh = false;
		leftEncoder->SetDistancePerPulse((6 * PI / 1000)/reduction); //6-inch wheels, 1000 raw counts per revolution,
		rightEncoder->SetDistancePerPulse((6 * PI / 1000)/reduction); //about 1:1 gear ratio
		DriverStationEnhancedIO &myEIO = driverStation->GetEnhancedIO();
		GetWatchdog().SetEnabled(true);
		GetWatchdog().SetExpiration(0.3);

		PIDDriveLeft->SetOutputRange(-1, 1);
		PIDDriveRight->SetOutputRange(-1, 1);
		//PIDDriveLeft->SetInputRange(-244,244);
		//PIDDriveRight->SetInputRange(-244,244);
		PIDDriveLeft->SetTolerance(5);
		PIDDriveRight->SetTolerance(5);
		PIDDriveLeft->SetContinuous(false);
		PIDDriveRight->SetContinuous(false);
		//PIDDriveLeft->Enable();
		//PIDDriveRight->Enable();
		PIDDriveRight->SetPID(DRIVEPROPGAIN, DRIVEINTGAIN, DRIVEDERIVGAIN);
		PIDDriveLeft->SetPID(DRIVEPROPGAIN, DRIVEINTGAIN, DRIVEDERIVGAIN);
		
		liftSP = 0;
		PIDLift->SetTolerance(10);
		PIDLift->SetContinuous(false);
		PIDLift->SetOutputRange(-0.75, 1.); //BUGBUG
		PIDLift->Enable();
		
		gripSP = 0;
		float goalGripSP = 0;
		bool useGoalSP = true;
		bool gripPIDOn = true;
		float gripP[10];
		float gripI[10];
		float gripD[10];
		PIDGrip->SetOutputRange(-0.5, 0.28); //Negative goes up
		PIDGrip->SetTolerance(5);
		PIDGrip->SetSetpoint(0);
		PIDGrip->Enable();
		miniDep->Set(miniDep->kForward);
		int i = 0;
		while(i < 10)
		{
			gripP[i] = GRIPPROPGAIN;
			i++;
		}
		i = 0;
		while(i < 10)
		{
			gripI[i] = GRIPINTGAIN;
			i++;
		}
		i = 0;
		while(i < 10)
		{
			gripD[i] = GRIPDERIVGAIN;
			i++;
		}

		while (IsOperatorControl())
		{
			GetWatchdog().Feed();
			count++;
#if !(SKELETON)
			sendVisionData();
#endif
			/*
			if(LIFTLOW1BUTTON && !(counts%10)) printf("LIFTLOW1BUTTON\n");
			if(LIFTLOW2BUTTON && !(counts%10)) printf("LIFTLOW2BUTTON\n");
			if(LIFTMID1BUTTON && !(counts%10)) printf("LIFTMID1BUTTON\n");
			if(LIFTMID2BUTTON && !(counts%10)) printf("LIFTMID2BUTTON\n");
			if(LIFTHIGH1BUTTON && !(counts%10)) printf("LIFTHIGH1BUTTON\n");
			if(LIFTHIGH2BUTTON && !(counts%10)) printf("LIFTHIGH2BUTTON\n");
			*/
			/*
			if(lsLeft->Get()) printf("LSLEFT\n");
			if(lsMiddle->Get()) printf("LSMIDDLE\n");
			if(lsRight->Get()) printf("LSRIGHT\n");
			*/
			stickY[0] = stickL.GetY();
			stickY[1] = stickR.GetY();
			stickYAbs[0] = fabs(stickY[0]);
			stickYAbs[1] = fabs(stickY[1]);
			if(bDrivePID)
			{
	#if 0
				frontLeftMotor->Set(stickY[0]);
				rearLeftMotor->Set(stickY[0]);
				frontRightMotor->Set(stickY[1]);
				rearRightMotor->Set(stickY[1]);
				
				if(!(count%5)) printf("Speeds: %4.2f %4.2f Outputs: %f %f \n", leftEncoder->GetRate(),
						rightEncoder->GetRate(), frontLeftMotor->Get(), frontRightMotor->Get());
	#endif		
				if(stickYAbs[0] <= 0.05 )
				{
					leftSP = 0;
					if(!(count%3) && !BACKWARDBUTTON)
					{
						PIDDriveLeft->Reset();
						PIDDriveLeft->Enable();
					}
				}
				else leftSP = stickY[0] * stickY[0] * (stickY[0]/stickYAbs[0]); //set points for pid control
				if(stickYAbs[1] <= 0.05)
				{
					rightSP = 0;
					if(!(count%3) && !BACKWARDBUTTON)
					{
						PIDDriveRight->Reset();
						PIDDriveRight->Enable();
					}
				}
				else rightSP = stickY[1] * stickY[1] * (stickY[1]/stickYAbs[1]);
				
				if (BACKWARDBUTTON)
				{
					tempRightSP = rightSP;
					tempLeftSP = leftSP;
					rightSP = -tempLeftSP;
					leftSP = -tempRightSP; //This line and above line sets opposite values for the controller. ...Theoretically.
				}
				
				PIDDriveLeft->SetSetpoint(leftSP);
				PIDDriveRight->SetSetpoint(rightSP);
					
				
				leftSpeed = leftEncoder->GetRate();
				rightSpeed = rightEncoder->GetRate();
				if(!(count++ % 5))
				{
				printf("rate L: %2.2f R: %2.2f SP %2.4f %2.4f ERR %2.2f %2.2f Pow: %1.2f %1.2f\n", 
						leftPIDSource->PIDGet(), rightPIDSource->PIDGet(), leftSP, rightSP,
						PIDDriveLeft->GetError(), PIDDriveRight->GetError(), frontLeftMotor->Get(),
						frontRightMotor->Get());
						
				
				//printf("Throttle value: %f", stickR.GetThrottle());
				if(PIDDriveRight->OnTarget()) printf("Right on \n");
				if(PIDDriveLeft->OnTarget()) printf("Left on \n");
				}
					
				if(PIDRESETBUTTON)
				{
					//PIDDriveRight->SetPID(stickR.GetThrottle()+1,DRIVEINTGAIN, DRIVEDERIVGAIN); 
					//PIDDriveLeft->SetPID(stickR.GetThrottle()+1,DRIVEINTGAIN, DRIVEDERIVGAIN);
					PIDDriveLeft->Reset();
					PIDDriveRight->Reset();
					PIDDriveLeft->Enable();
					PIDDriveRight->Enable();
				}
			}
			else
			{
				if(PIDDriveLeft->IsEnabled()) PIDDriveLeft->Reset();
				if(PIDDriveRight->IsEnabled()) PIDDriveRight->Reset();
				if(DEMOSWITCH)
				{
					stickY[0] = stickY[0]*(1 - lift->getPosition()); //reduces power based on lift height
					stickY[1] = stickY[0]*(1 - lift->getPosition());
				}
				if(stickYAbs[0] > 0.05)
				{
					frontLeftMotor->Set(stickY[0]);
					rearLeftMotor->Set(stickY[0]);
				}
				else
				{
					frontLeftMotor->Set(0);
					rearLeftMotor->Set(0);
				}
				if(stickYAbs[1] > 0.05)
				{
					frontRightMotor->Set(-stickY[1]);
					rearRightMotor->Set(-stickY[1]);
				}
				else
				{
					frontRightMotor->Set(0);
					rearRightMotor->Set(0);
				}
			}
			
			if(stickL.GetRawButton(2) && stickL.GetRawButton(3) && stickR.GetRawButton(2) &&
					stickR.GetRawButton(3) && BACKWARDBUTTON && !(count%5)) bDrivePID = !bDrivePID;
			
			if((SHIFTBUTTON && shiftHigh) || DEMOSWITCH)
			{
				shifter->Set(shifter->kReverse);
				shiftHigh = false;
				maxSpeed = 12;
			}
			else if(!SHIFTBUTTON && !shiftHigh && !DEMOSWITCH)
			{
				shifter->Set(shifter->kForward);
				shiftHigh = true;
				maxSpeed = 25; //last value 35
			}

			sendIOPortData();
#if !(SKELETON)
			/*
			if(LIGHTBUTTON) lightOn = true;
			else lightOn = false;
			if(!lightOn) light->Set(light->kOff);
			if(lightOn) light->Set(light->kOn);
			*/
			if(!MODESWITCH)
			{
				lastLiftSP = liftSP;
				if(!PIDLift->IsEnabled()) PIDLift->Enable();
				if(LIFTLOW1BUTTON) liftSP = LIFTLOW1;
				if(LIFTLOW2BUTTON) liftSP = LIFTLOW2;
				if(LIFTMID1BUTTON) liftSP = LIFTMID1;
				if(LIFTMID2BUTTON) liftSP = LIFTMID2;
				if(LIFTHIGH1BUTTON) liftSP = LIFTHIGH1;
				if(LIFTHIGH2BUTTON && !(DEMOSWITCH && (stickYAbs[0] > 0.05 || stickYAbs[1] > 0.05))) liftSP = LIFTHIGH2;
				
				if(!lift->isBraking() && !disengageBrake)
				{
					PIDLift->SetSetpoint(liftSP);
					if(liftSP == 0 && liftPIDSource->PIDGet() < 0.1) //BUGBUG
					{
						//PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, 3*LIFTDERIVGAIN);
						PIDLift->SetOutputRange(-liftPIDSource->PIDGet() - 0.1, 1.); //BUGBUG
					}
					else PIDLift->SetOutputRange(-0.75, 1.); //BUGBUG
				}
				if(lift->isBraking() && lastLiftSP != liftSP)
				{
					PIDLift->SetSetpoint(lastLiftSP + 0.06);
					PIDLift->SetPID(12.5 + 1.5*lift->getPosition(), LIFTINTGAIN + 0.6 + 3*lift->getPosition()/10, 0);
					lift->brakeOff();
					disengageBrake = true;
					if(!liftCount) liftCount = count;
				}
				//set brake (because near)
				if(fabs(PIDLift->GetError()) < 0.01 && !lift->isBraking() && !disengageBrake)
				{
					if(liftCount == 0) liftCount = count;
					if(count - liftCount > 3)
					{
						PIDLift->Reset();
						liftMotor1->Set(LIFTNEUTRALPOWER);
						liftMotor2->Set(LIFTNEUTRALPOWER);
						Wait(0.02);
						lift->brakeOn();
						Wait(0.02);
						liftMotor1->Set(0.0);
						liftMotor2->Set(0.0);
						PIDLift->Enable();
						PIDLift->SetSetpoint(lift->getPosition());
						liftCount = 0;
					}
					//if(!(count%50)) printf("Braking/Not PID \n");
				}
				if(lift->isBraking() && !(count%10)) PIDLift->SetSetpoint(lift->getPosition());
				if(fabs(PIDLift->GetError()) < 0.01 && disengageBrake && count - liftCount > 3)
				{
					disengageBrake = false;
					if(liftEncoder->PIDGet() < liftSP) PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN - 0.015);
					else PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN + 0.015);
					liftCount = 0;
				}
				
				//pid
				/*
				else if(!(fabs(PIDLift->GetError()) < 0.04) && !lift->isBraking() && liftPos == -20)
				{
					PIDLift->Enable();
					liftPos = -10;
					printf("PID GO PID GO PID GO PID GO PID GO \n");
				}
				*/
				//when liftPos is positive, measures position
				//when liftPos = -10, is pidding
				//when liftPos = -20, has just released brake
			}
			else //(MODESWITCH)
			{
				if(PIDLift->IsEnabled()) PIDLift->Reset();
				liftPower = .8*pow(2*((log(LIFTSLIDER + 0.003/.0208116511)/LOG17) + 0.116), 2)*(2*((log(LIFTSLIDER + 0.003/.0208116511)/LOG17) + 0.116)/fabs(2*((log(LIFTSLIDER + 0.003/.0208116511)/LOG17) + 0.116)));
				liftPowerAbs = fabs(liftPower);
				if(liftPowerAbs > 1) liftPower /= liftPowerAbs;
				//if(!(count%5)) printf("Slider: %f", liftPower);
				
				if(lift->isBraking() && liftPowerAbs > 0.05) lift->brakeOff();
				else if(!lift->isBraking() && liftPowerAbs <= 0.05 && !(count%5)) lift->brakeOn();
				if (liftPowerAbs > 0.05)
				{
					liftMotor1->Set(liftPower);
					liftMotor2->Set(liftPower);
				}
				else
				{
					liftMotor1->Set(0);
					liftMotor2->Set(0);
				}
			}
			if(MODESWITCH && LIFTLOW1BUTTON && LIFTMID1BUTTON && LIFTHIGH1BUTTON) liftEncoder->Reset();
			/*
			if(!(count%5))
			{
				printf("Lift pos: %f Lift error: %f Lift SP: %f \n", liftPIDSource->PIDGet(),
						PIDLift->GetError(), PIDLift->GetSetpoint());
			}
			*/
			if(!(count%5) && MODESWITCH && GRIPPERPOSUP && GRIPPERPOSDOWN && GRIPPERPOP)
			{	
				gripPIDOn = !gripPIDOn;
				printf("Switch'd\n");
			}
			if(gripPIDOn)	
			{
				if(!PIDGrip->IsEnabled()) PIDGrip->Enable();
				if(GRIPPERPOSUP && !GRIPPERPOSDOWN)
				{
					goalGripSP = 0;
				}
				else if(GRIPPERPOSDOWN && !GRIPPERPOSUP && lift->getPosition() < 0.5)
				{
					goalGripSP = 1;
				}
				/*
				else if(!GRIPPERPOSDOWN && !GRIPPERPOSUP)
				{
					goalGripSP = 0.5;
				}
				*/
				
				gripError = PIDGrip->GetError();
				gripErrorAbs = fabs(gripError);
				PIDGrip->SetSetpoint(goalGripSP);
				
				if(gripErrorAbs < 0.4) PIDGrip->SetOutputRange(-0.4, 0.6); //negative is up
				else PIDGrip->SetOutputRange(-0.9, 0.8); //negative is up
				if(gripErrorAbs > 0.0 && gripErrorAbs < 0.2)
				{
					PIDGrip->SetPID(GRIPPROPGAIN - 1.25*(1 - gripErrorAbs) + gripPropMod, GRIPINTGAIN + gripIntMod, 0.3 + 0.1*(1 - gripPIDSource->PIDGet()));
				}
				else
				{
					PIDGrip->SetPID(GRIPPROPGAIN - 1.*(1 - gripErrorAbs) + gripPropMod, 0, 0.02);
				}
				if(fabs(gripPIDSource->PIDGet()) < 0.03 && PIDGrip->GetSetpoint() == 0)
				{
					gripLatch1->Set(true);
					gripLatch2->Set(false);
				}
				else if(!(gripLatch1->Get() && PIDGrip->GetSetpoint() == 0) || 
						gripPIDSource->PIDGet() < 0) 
				{
					gripLatch1->Set(false);
					gripLatch2->Set(true);
				}
					
				if(gripLatch1->Get() && !(count%20)) 
				{
					PIDGrip->Reset();
					PIDGrip->Enable();
				}
				/*
				if(stickL.GetRawButton(1) && !(count%5)){
					gripIntMod -= 0.001;
				}
				
				if(stickR.GetRawButton(1) &&!(count%5))
				{
					gripIntMod += 0.001;
				}
				if(stickL.GetRawButton(2) && !(count%5))
				{
					gripPropMod -= 0.1;
				}
				if(stickL.GetRawButton(3) && !(count%5))
				{
					gripPropMod += 0.1;
				}
				*/
				/*
				if(LIFTBOTTOMBUTTON)
				{
					PIDGrip->Reset();
					PIDGrip->Enable();
				}
				*/
				
				if(!(count%5))
				{
					printf("Gripper pos: %f Gripper error: %f Grip power: %f \n",
							gripPIDSource->PIDGet(), PIDGrip->GetError(), gripMotor1->Get());
				}
				
			
			}

			//Calibration routine
			else
			{
				if(gripLatch1->Get() == true) 
				{
					gripLatch1->Set(false);
					gripLatch2->Set(true);
				}
				if(PIDGrip->IsEnabled()) PIDGrip->Reset();
				if(GRIPPERPOSUP)
				{
					gripMotor1->Set(-0.5);
					//gripMotor2->Set(0.5);
				}
				else if(GRIPPERPOSDOWN)
				{
					gripMotor1->Set(0.5);
					//gripMotor2->Set(-0.5);
					
				}
				else
				{
					gripMotor1->Set(0);
					//gripMotor2->Set(0);
				}
			}
			//if(!(count%5)) printf("Grip volts: %f \n", gripPot->GetVoltage());
			//if(!(count%5)) printf("Grip 1 voltage: %f \n", gripMotor1->Get());
			if(GRIPPEROPEN && !popStage)
			{
#if !(INNERGRIP)
				gripOpen1->Set(true);
				gripOpen2->Set(false);
#else
				gripOpen1->Set(false);
				gripOpen2->Set(true);
#endif
			}
			else if(!popStage)
			{
#if !(INNERGRIP)
				gripOpen1->Set(false);
				gripOpen2->Set(true);
#else
				gripOpen1->Set(true);
				gripOpen2->Set(false);
#endif
			}
			if(GRIPPERPOP && !popStage && goalGripSP == 0 && !(GRIPPEROPEN && GRIPPERCLOSE)) popStage = 1;
			if(popStage) popTime = GetClock();
			if(popStage == 1)
			{
				//popCount = count;
				popStart = GetClock();
				popStage++;
				//printf("POP START POP START POP START \n");
			}
			if(popStage == 2)
			{
#if !(INNERGRIP)
				gripOpen1->Set(true);
				gripOpen2->Set(false);
#else
				gripOpen1->Set(false);
				gripOpen2->Set(true);
#endif
				popStage++;
				//printf("GRIP OPEN GRIP OPEN GRIP OPEN \n");
			}
			if(popStage == 3 && popTime - popStart > 0.0) //used to be 0.15
			{
				gripPop1->Set(true);
				gripPop2->Set(false);
				popStage++;
				//printf("POP OUT POP OUT POP OUT \n");
			}
			if(popStage == 4 && popTime - popStart > .75) //time was 0.9
			{
				gripPop1->Set(false);
				gripPop2->Set(true);
				popStage++;
				//printf("POP IN POP IN POP IN \n");
			}
			if(popStage == 5 && popTime - popStart > 0.9)	popStage = 0; //time was 1.05
			
			if(MINIBOTSWITCH && !(MODESWITCH && stickR.GetRawButton(1) && stickL.GetRawButton(1))) miniDep->Set(miniDep->kReverse);
			else if(MINIBOTSWITCH && MODESWITCH && stickR.GetRawButton(1) && stickL.GetRawButton(1)) miniDep->Set(miniDep->kOn);
#endif			
			if(!compSwitch->Get()) compressor->Set(compressor->kReverse);
			else compressor->Set(compressor->kOff);
			/*
			if(stickR.GetRawButton(1)) compressor->Set(compressor->kReverse);
			else compressor->Set(compressor->kForward);
			*/
			Wait(0.02);				// wait for a motor update time
		}
	}
Пример #2
0
	void OperatorControl()
	{
		compressor.Start();
		myRobot.SetSafetyEnabled(true);
		myRobot.SetInvertedMotor(myRobot.kRearLeftMotor, true);//Invert rear left Motor
		myRobot.SetInvertedMotor(myRobot.kRearRightMotor, true);//Invert rear right motor
		myRobot.SetInvertedMotor(myRobot.kFrontLeftMotor, true);//Invert rear right motor
		myRobot.SetInvertedMotor(myRobot.kFrontRightMotor, true);
		
		DriverStation *ds;
		DriverStationLCD *dsLCD;
		ds = DriverStation::GetInstance();
		dsLCD = DriverStationLCD::GetInstance();
		
		dsLCD->Printf(DriverStationLCD::kUser_Line1,1, "Starting Teleop");
		dsLCD->UpdateLCD();
		
		while (true)
		{
			if (!compressor.GetPressureSwitchValue()){
				compressor.Start();
			}
			myRobot.ArcadeDrive(stick); 
			
			 /*PNEUMATIC CODE*/
			if (stick.GetRawButton(8)){
				shooterSole.Set(shooterSole.kForward);
			}
			if (stick.GetRawButton(1)){
				shooterSole.Set(shooterSole.kReverse);
			}
			 /*SHOOTER CODE*/
			if (stick.GetRawButton(2)){
				shooter.SetSpeed(1);
			}
			if (stick.GetRawButton(4)){
				shooter.SetSpeed(-1);
				dsLCD->Printf(DriverStationLCD::kUser_Line1,(int)forklift.Get(), "Shooter Should be negative");
				dsLCD->UpdateLCD();
			}
			if (!stick.GetRawButton(4) || !stick.GetRawButton(2)){
				shooter.SetSpeed(0);
			}
			 /* FORKLIFT CODE*/
			if (!stick.GetRawButton(5) || !stick.GetRawButton(6)){
				forklift.SetSpeed(0);
			} 
			if (stick.GetRawButton(5)){
				forklift.SetSpeed(1);
			}
			if (stick.GetRawButton(6)){
				forklift.SetSpeed(-1);
				dsLCD->Printf(DriverStationLCD::kUser_Line1,(int)forklift.Get(), "Forklift Should be negative");
				dsLCD->UpdateLCD();
			}
			if (!shoot.Get()){
				shooter.SetSpeed(0);
				shooterSole.Set(shooterSole.kForward);
			}
			
			
			if (stick.GetRawButton(11)){
				//myRobot.m_rearLeftMotor.SetSpeed(1);
				//myRobot.m_rearRightMotor.SetSpeed(1);
				//myRobot.m_frontLeftMotor.SetSpeed(1);
				//myRobot.m_frontRightMotor.SetSpeed(1);
			}
			//Wait(0.005);// wait for a motor update time
		}
	}