void
Elevator::PeriodicService()
{
	// For now, add a test for NULL for our motors. Until they are connected to to the bot,
	// we can't do anything
	if ((motorA == NULL) || (motorB == NULL)) return;
	
	CheckJoystick();
	
	if(isHoming)
	{
		if(motorA->GetReverseLimitOK())
		{
			motorA->Set(-maxSpeed/2); //both move down at half speed, can change to accomodate for others
			motorB->Set(-maxSpeed/2);
			return;
		}
		isHoming = false;
		motorA->Set(0);
		motorB->Set(0);
		motorA->ResetEncoder();
		targetPosition = 0;
	}
	
		
	
	float positionNow = GetCurrentPositionFeet();
	
	float err = (targetPosition - positionNow);
	
	float output = err*Kp;
	
	if(IsAtTargetPosition()) //brakes if at the target position
		SetBrake(true);
	else
		SetBrake(false);
	
	if(output > maxSpeed) //makes sure the elevator doesn't exceed a set max speed
		output = maxSpeed;
	if(output < -maxSpeed)
		output = -maxSpeed;
	
	motorA->Set(output);
	motorB->Set(output);
	
}
Ejemplo n.º 2
0
void RevLimUnitTests()
{
	ClearRam();
	//Run initializer
	PopulateRamVariables();
	
#if VIN_HACKS
	//Bypass VIN Cut
	pRamVariables->VinAuth = 0x01;
#endif

	SetBrake(0);
	SetClutch(0);

	////
	//Redline Test
	///////////////
	//Clear flag
	*pFlagsRevLim = 0x00;
	//Set just below REDLINE, should not cut
	*pEngineSpeed = pRamVariables->RedLineCut - 1;
	*pVehicleSpeed = 19.0f;
	SetClutch(0);
	RevLimCode();
	Assert(!GetFuelCutFlag(), "RedLine: Allow fuel at RedLineCut - 1 RPM, moving, clutch not pressed !FuelCut");
	Assert(!pRamVariables->LCEngaged, "RedLine: Allow fuel at RedLineCut - 1 RPM, moving, clutch not pressed, !LCEngaged");
	
	//Set above REDLINE, should cut
	*pEngineSpeed = pRamVariables->RedLineCut + 1;
	*pVehicleSpeed = 19.0f;
	SetClutch(0);
	RevLimCode();
	Assert(GetFuelCutFlag() && !pRamVariables->LCEngaged, "RedLine: Cut fuel at RedLineCut + 1 RPM, moving, clutch not pressed");	

	//Set back below hysteresis, should RESUME
	*pEngineSpeed = pRamVariables->RedLineCut - pRamVariables->RedLineHyst - 1;
	*pVehicleSpeed = 19.0f;
	SetClutch(0);
	RevLimCode();
	Assert(!GetFuelCutFlag() && !pRamVariables->LCEngaged, "RedLine: Resume fuel at RedLineResume - 1 RPM, moving, clutch not pressed");
	
	////
	//LC Tests
	///////////
	
	//TEST 1: Stationary car
	//Clutch is out
	//RPM is between LC limit and Redline Limit
	//Should NOT CUT
	*pThrottlePlate = 100;
	*pEngineSpeed = pRamVariables->LaunchControlCut + 1000;
	*pVehicleSpeed = 0.0f;
	RevLimCode();
	//-----V this ! means it SHOULD NOT cut
	Assert(!GetFuelCutFlag() && !pRamVariables->LCEngaged, "Normal stopped: Allow fuel at LaunchControlCut + 1000 RPM, stopped, no clutch");
	

	//TEST 2: Repeat test 1 with vehicle speed
	//Should NOT CUT
	*pEngineSpeed = pRamVariables->LaunchControlCut + 1000;
	*pVehicleSpeed = 19.0f;
	SetClutch(0);
	RevLimCode();
	Assert(!GetFuelCutFlag() && !pRamVariables->LCEngaged, "Normal moving: Allow fuel at LaunchControlCut + 1000 RPM, moving, no clutch");
	
	//TEST 3: Stationary car
	//Clutch in
	//RPM Below limits
	//Shoudl NOT CUT
	*pEngineSpeed = pRamVariables->LaunchControlCut - 1;
	*pVehicleSpeed = 0.0f;
	*pThrottlePlate = 10.0f;
	SetClutch(1);
	RevLimCode();
	Assert(!GetFuelCutFlag() && !pRamVariables->LCEngaged, "Launch Control: Allow fuel at LaunchControlCut - 1 RPM, standstill, clutch pressed");
	
	//TEST 4: Repeat test 3
	//Set above LC limit
	//SHOULD CUT FUEL HERE!!
	*pThrottlePlate = 100.0f;
	*pEngineSpeed = pRamVariables->LaunchControlCut + 1;
	*pVehicleSpeed = 0.0f;
	SetClutch(1);
	RevLimCode();
	Assert(GetFuelCutFlag() && pRamVariables->LCEngaged, "Launch Control: Cut fuel at LaunchControlCut + 1 RPM, standstill, clutch pressed");
	
	//Test 5: Repeat test 4
	//Set RPM below limit, but above resume value
	//SHOULD NOT RESUME YET!
	*pEngineSpeed = pRamVariables->LaunchControlCut - 1;
	*pVehicleSpeed = 0.0f;
	SetClutch(1);
	RevLimCode();
	Assert(GetFuelCutFlag() && pRamVariables->LCEngaged, "Launch Control: Resume fuel at LaunchControlResume - 1 RPM, standstill, clutch pressed");
	
	//TEST 6: Repeat test 5
	//Set below LC hysteresis
	//SHOULD RESUME HERE, but LC still engaged (on LC limit)
	*pEngineSpeed = pRamVariables->LaunchControlCut - pRamVariables->LaunchControlHyst - 1;
	*pVehicleSpeed = 0.0f;
	SetClutch(1);
	RevLimCode();
	int shit = !GetFuelCutFlag();
	shit &= pRamVariables->LCEngaged;
	Assert(shit , "Launch Control: Resume fuel at LaunchControlResume - 1 RPM, standstill, clutch pressed");
	
	//TEST7: low throttle
	//Set throttle 
	*pEngineSpeed = pRamVariables->RedLineCut - 200;
	*pVehicleSpeed = 0.0f;
	*pThrottlePlate = LCMinimumThrottle + 1;
	SetClutch(1);
	RevLimCode();
	//should cut here
	Assert(GetFuelCutFlag() && pRamVariables->LCEngaged, "Launch Control: Resume fuel at LaunchControlResume - 1 RPM, standstill, clutch pressed");
	*pThrottlePlate -= 10;
	RevLimCode();
	Assert(!GetFuelCutFlag() && !pRamVariables->LCEngaged, "Launch Control: Resume fuel at LaunchControlResume - 1 RPM, standstill, clutch pressed");
	//should resume here
	
		
#if LC_ADJUST
	//Test LC adjust
	//Setup:
	*pEngineSpeed = pRamVariables->RedLineCut - 200;
	*pVehicleSpeed = 0.0f;
	*pThrottlePlate = LCMinimumThrottle + 1;
	SetClutch(1);
	RevLimCode();
	//should cut here
	Assert(GetFuelCutFlag() && pRamVariables->LCEngaged, "Launch Control: Resume fuel at LaunchControlResume - 1 RPM, standstill, clutch pressed");
	//Adjust
	*pEngineSpeed = pRamVariables->LaunchControlCut - pRamVariables->LaunchControlHyst + 5; //Still should cut
	Assert(GetFuelCutFlag() && pRamVariables->LCEngaged, "Launch Control: Resume fuel at LaunchControlResume - 1 RPM, standstill, clutch pressed");
	*pResumeFlags |= ResumeBitMask;
	//increases limit
	WGDCHack();
	RevLimCode();
	//should resume as limit has increased
	Assert(pRamVariables->LCEngaged, "Launch Control: Resume fuel at LaunchControlResume - 1 RPM, standstill, clutch pressed, !LCEngaged");
	Assert(!GetFuelCutFlag(), "Launch Control: Resume fuel at LaunchControlResume - 1 RPM, standstill, clutch pressed !FuelCut");
	*pResumeFlags = 0x00;
	*pCoastFlags |= CoastBitMask;
	WGDCHack();
	RevLimCode();
	//should cut again
	Assert(pRamVariables->LCEngaged, "Launch Control: Resume fuel at LaunchControlResume - 1 RPM, standstill, clutch pressed, !LCEngaged");
	Assert(!GetFuelCutFlag(), "Launch Control: Resume fuel at LaunchControlResume - 1 RPM, standstill, clutch pressed !FuelCut");
#endif
	
	//////
	//FFS TESTS
	////////////////
	
	//TEST 1: Check that low throttle disables the cut
	//Set pre-shift conditions
	*pCurrentGear = 4;
	*pThrottlePlate = 20;
	*pVehicleSpeed = 50.0f;
	*pEngineSpeed = pRamVariables->LaunchControlCut - 100;
	SetClutch(0);
	RevLimCode(); //This sets the current gear
	SetClutch(1);
	RevLimCode();
	//Should NOT cut due to throttle.
	Assert(!GetFuelCutFlag() && !pRamVariables->LCEngaged, "Flat Foot Shifting: Resume fuel at FlatFootShiftResume - 1 RPM, moving, clutch pressed");

	//TEST 2: Set throttle high
	//Should engage FFS and cut
	*pEngineSpeed = pRamVariables->FlatFootShiftRpmThreshold + 500;
	*pThrottlePlate = FFSMinimumThrottle + 1;
	RevLimCode(); //This sets the FFS engaged, and sets the FFSRPM
	Assert(GetFuelCutFlag() && (pRamVariables->FFSEngaged == 2), "Flat Foot Shifting: Cut fuel at FlatFootShiftCut + 1 RPM, moving, clutch pressed");	

	//TEST 3: Set rpm low
	//Should disable FFS and resume fuel
	*pEngineSpeed = pRamVariables->RevLimResume - 1;
	RevLimCode();
	Assert(!GetFuelCutFlag() && !pRamVariables->FFSEngaged, "FFS resume when rpm drops");
	
	//TEST 4: Engage ffs and cut again
	*pEngineSpeed = pRamVariables->FlatFootShiftRpmThreshold + 500;
	RevLimCode(); //This sets the FFS engaged, and sets the FFSRPM
	RevLimCode(); //This runs the FFS limiter, and SHOULD CUT
	Assert(GetFuelCutFlag() && pRamVariables->FFSEngaged, "Flat Foot Shifting: Cut fuel at FlatFootShiftCut + 1 RPM, moving, clutch pressed");	
	//Set throttle below  FFS threshold, should resume
	*pThrottlePlate = FFSMinimumThrottle - 1;
	*pVehicleSpeed = 19.0f;
	SetClutch(1);
	RevLimCode();
	RevLimCode();
	Assert(!GetFuelCutFlag(), "Flat Foot Shifting: Resume fuel at FlatFootShiftResume - 1 RPM, moving, clutch pressed");
	*pThrottlePlate = 100;
	

	
	// Verify the other bits in the rev limiter flag are not modified.
	// ONLY NECESSARY FOR BITMASKING
	
	*pEngineSpeed = 6000.0f;
	*pVehicleSpeed = 0.0f;
	SetClutch(1);
	SetBrake(0);
	*pFlagsRevLim = 0x00;					
	RevLimCode();
	unsigned char flags = *pFlagsRevLim;
	Assert(flags == (unsigned char) RevLimBitMask, "When rev limit flag is set, no other bits are set.");

	// Verify the other bits in the rev limiter flag are not modified.
	*pEngineSpeed = 1000.0f;
	*pVehicleSpeed = 0.0f;
	SetClutch(0);
	*pFlagsRevLim = 0xFF;
	RevLimCode();
	flags = ~*pFlagsRevLim;
	Assert( flags == (unsigned char) RevLimBitMask, "When rev limit flag is cleared, no other bits are cleared.");
		
	// Sanity check the defaults
	Assert(pRamVariables->RedLineCut > pRamVariables->RedLineCut - pRamVariables->RedLineHyst, "RedLine cut/resume sanity.");
	Assert(pRamVariables->LaunchControlCut > pRamVariables->LaunchControlCut - pRamVariables->LaunchControlHyst, "LaunchControl cut/resume sanity.");
}