Last active
June 29, 2018 21:13
-
-
Save 9185/673f617a824e8b33ec275f2a6b113c09 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// === 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); | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment