NBN9185.c

// === Just some stuff ===

/*==========
4 Ball Auton
==========*/
void ShootPreloads()
{
  // Clear old data
  datalogClear();

	JumperChange = -1 + SensorValue[Plus1] + SensorValue[Plus2]*2 + SensorValue[Plus3]*3 + SensorValue[Plus4]*4
								 + SensorValue[Plus5]*5 + SensorValue[Plus6]*6 - SensorValue[Minus2]*2 - SensorValue[Minus3]*3
								 - SensorValue[Minus4]*4 - SensorValue[Minus5]*5 - SensorValue[Minus6]*6;
	JumperChange = -1*JumperChange;

	startTask(ControlTask);							//Flywheel velocity control

	clearLCDLine(0);										//Clears LCD

	bLCDBacklight = true;								//LCD backlight

	nMotorEncoder[LeftFront] = 0;
	nMotorEncoder[Fly1] = 0;

	VelocitySet(155,FULLSETTING+JumperChange);								//Sets flywheel to 155 RPM
  Range = 4;

	sleep(2500);												//Accelerate for 2.5 seconds

	MoveLift(UP);

	LastSysTime = nSysTime;							//Save old system time

	int a = 0;
	while((a<3)||(TimeElapsed < 5000))	//Run only for a maximum of 5 seconds (I know, weird time elapsed thing)
	{
		while((FullSpeed == 0)&&(TimeElapsed < 5000))
		{
			MoveFeeder();
			sleep(25);
			TimeElapsed = nSysTime - LastSysTime;
		}
		while((FullSpeed == 1)&&(TimeElapsed < 5000))
		{
			StopFeeder();
			MoveLift(SLOW);
			sleep(25);
			TimeElapsed = nSysTime - LastSysTime;
		}
		wait1Msec(100);
		a++;
	}
	StopFeeder();
	StopLift();
	VelocitySet(0,0);
}

/*=============
RPM Adjustments
=============*/
if(vexRT[Btn7U]==1)					//Sets Flywheel To Full Court
{
	CurrentSettingTarget = 155;
	CurrentSettingPredicted = FULLSETTING+JumperChange;
	VelocitySet(CurrentSettingTarget, CurrentSettingPredicted);
	Adjustment = 0;
	Range = 4;
}

if(vexRT[Btn7R]==1)					//Sets Flywheel To 3/4 Court
{
	CurrentSettingTarget = 125;
	CurrentSettingPredicted = THREEQSETTING+(JumperChange*2/3);
	VelocitySet(CurrentSettingTarget, CurrentSettingPredicted);
	Adjustment = 0;
	Range = 3;
}

if(vexRT[Btn7D]==1)					//Sets Flywheel To 1/2 Court
{
	CurrentSettingTarget = 121;
	CurrentSettingPredicted = HALFSETTING+(JumperChange/2);
	VelocitySet(CurrentSettingTarget, CurrentSettingPredicted);
	Adjustment = 0;
	Range = 2;
}

if(vexRT[Btn8L]==1)					//Sets Flywheel To 1/4 Court
{
	CurrentSettingTarget = 100;
	CurrentSettingPredicted = QUARTERSETTING;
	VelocitySet(CurrentSettingTarget, CurrentSettingPredicted);
	Adjustment = 0;
	Range = 1;
}

if(vexRT[Btn8U]==1)					//Sets Flywheel To 0
{
	CurrentSettingTarget = 0;
	CurrentSettingPredicted = 0;
	VelocitySet(CurrentSettingTarget, CurrentSettingPredicted);
	Adjustment = 0;
	Range = 0;
  }

if(vexRT[Btn8R]==1)
{
	if(vexRT[Btn5U]==1)
	{
		Adjustment = Adjustment + 1;
		wait1Msec(100);
		VelocitySet(CurrentSettingTarget+Adjustment, CurrentSettingPredicted);
	}
	if(vexRT[Btn5D]==1)
	{
		Adjustment = Adjustment - 1;
		wait1Msec(100);
		VelocitySet(CurrentSettingTarget+Adjustment, CurrentSettingPredicted);
	}
}

/*=========================================================
Bang Bang + Some PI+C (Based off of jpearman's TBH control)
=========================================================*/
//Flywheel Definitions
bool FullSpeed;										//Holds Logic if Flywheel is At Full
long FullSpeedStartTime;

int Range;

#define LOOP_SPEED 25							//Update inteval (in mS) for the flywheel control loop

long EncoderCounts;								//Current Encoder Value
long EncoderCountsLast;						//Last Encoder Value

float MotorVelocity;							//Current Velocity in RPM
long encoder_timestamp;
long encoder_timestamp_last;			//Time of Last Velocity Calculation

long TargetVelocity;       				//Target RPM

float CurrentError;      			    //Current Error Between Target and Actual RPMs
float LastError;          			  //Last Error
float AverageError;								//Average of Current Error and Last Error

float FilteredError;							//Average of Current and Last Three Errors
float BatteryAdjustmentFull;			//Adjustment of Predicted Motor Power For Full
float BatteryAdjustmentThreeQ;		//								 "									For Three Quarters
float BatteryAdjustmentHalf;			//								 "									For Half
float BatteryAdjustmentQuarter;		//								 "									For Quarter
float BatteryAdjustment;					//								 "									In Current Setting

float ProportionalFw;
float IntegralFw;
float kP_Fw = 1.5;
float kI_Fw = .7;
bool PIControlOn = false;

int Predicted;										//Predicted Motor Value

void Flywheel(int Speed)					//Sets Flywheel Speed
{
	motor[Fly1] = Speed;
	motor[Fly2] = Speed;
	motor[Fly3] = Speed;
}

void CalculateSpeed()
{
	int delta_ms;
	int delta_enc;

  // Get current encoder value and time
  getEncoderAndTimeStamp(Fly1, EncoderCounts, encoder_timestamp);

  // This is just used so we don't need to know how often we are called
  // how many mS since we were last here
  delta_ms = encoder_timestamp - encoder_timestamp_last;
  encoder_timestamp_last = encoder_timestamp;

	//Change in encoder count
	delta_enc = (EncoderCounts - EncoderCountsLast);

	//Save last position
	EncoderCountsLast = EncoderCounts;

	//Calculate velocity in rpm
	MotorVelocity = (1000.0 / (delta_ms+0.0001)) * delta_enc * 60.0 / 392;
}

void ControlUpdateVelocity()
{
	// calculate error in velocity
	// target_velocity is desired velocity
	// current is measured velocity
	LastError = CurrentError;
	CurrentError	= TargetVelocity - MotorVelocity;
	AverageError = LastError*.5 + CurrentError*.5;

	ProportionalFw = AverageError * kP_Fw;
	IntegralFw = (IntegralFw + AverageError) * kI_Fw;

	if(CurrentError > 20)
	{
		IntegralFw = 0;
		kP_Fw = 1.5;
	}
	if(CurrentError < 5)
 {
		kP_Fw = .7;
	}

	FilteredError = CurrentError*.25 + LastError*.75;		//This is for automatic adjustment
}

task ControlTask()
{
	while(1)
	{
		// Calculate velocity
		CalculateSpeed();

		ControlUpdateVelocity();

		// and finally set the motor control value
		// Keeps previous value if CurrentError is between 5 and 20
		if(Range == 0)
		{
			Flywheel(0);
		}

		if((Range == 1)||(Range == 2)||(Range == 3))
		{
			if(FullSpeed)
			{
				FullSpeedStartTime = nSysTime;
			}
			if(AverageError<5)
			{
				FullSpeed = false;
				if(Range==3)
				{
					Flywheel(Predicted+BatteryAdjustmentThreeQ);
				}
				if(Range==2)
				{
					Flywheel(Predicted+BatteryAdjustmentHalf);
				}
				if(Range==1)
				{
					Flywheel(Predicted+BatteryAdjustmentQuarter);
				}
			}
		}

		else if(Range>3)
		{
			if(PIControlOn)
			{
				FullSpeed = false;
				Flywheel(ProportionalFw+IntegralFw+Predicted);
			}
			else
			{
				if(FullSpeed)
				{
					FullSpeedStartTime = nSysTime;
				}
				if(AverageError<5)
				{
					FullSpeed = false;
					Flywheel(Predicted+BatteryAdjustmentFull);
				}
			}
		}

		if(AverageError>20)
		{
			Flywheel(127);
			FullSpeed = true;
		}

  	if((!FullSpeed)&&(!Range==0))
  	{
  		if((nSysTime-FullSpeedStartTime)>1000)
  		{
  			if(Range == 4)
  			{
					if(FilteredError>5)
					{
						BatteryAdjustmentFull += .2;
						BatteryAdjustment = BatteryAdjustmentFull;
					}
					else if(FilteredError<(-5))
					{
						BatteryAdjustmentFull -= .2;
						BatteryAdjustment = BatteryAdjustmentFull;
					}
  			}
  			else if(Range == 3)
  			{
					if(FilteredError>5)
					{
						BatteryAdjustmentThreeQ += .1;
						BatteryAdjustment = BatteryAdjustmentThreeQ;
					}
					else if(FilteredError<(-5))
					{
						BatteryAdjustmentThreeQ -= .1;
						BatteryAdjustment = BatteryAdjustmentThreeQ;
					}
  			}
  			else if(Range == 2)
  			{
					if(FilteredError>5)
					{
						BatteryAdjustmentHalf += .1;
						BatteryAdjustment = BatteryAdjustmentHalf;
					}
					else if(FilteredError<(-5))
					{
						BatteryAdjustmentHalf -= .1;
						BatteryAdjustment = BatteryAdjustmentHalf;
					}
  			}
  			else if(Range == 1)
  			{
					if(FilteredError>5)
					{
						BatteryAdjustmentQuarter += .1;
						BatteryAdjustment = BatteryAdjustmentQuarter;
					}
					else if(FilteredError<(-5))
					{
						BatteryAdjustmentQuarter -= .1;
						BatteryAdjustment = BatteryAdjustmentQuarter;
					}
  			}
			}
		}

		// Run at somewhere between 20 and 50mS
		wait1Msec(LOOP_SPEED);
	}
}