Here is the actual code. “autopick” is set in driver control by the LCD for switching among autonomous modes.
task TBHFlywheel()//Take Back Half controller with Integral accumulator for live adjustment
{//Used for the first time successfully after a series of breakthroughs using the graphing and analysis ability
//through debug stream and tools in Excel.
//Finally got RPM calculation right for both encoders, and also got TBH to work over snow break from 1-20 to 1-29
motor[launcher4] = 0;
motor[launcher3] = 0;
motor[launcher2] = 0;//stop flywheel ahead of time.
motor[launcher1] = 0;
//motor[launcher5] = 0;
//motor[launcher6] = 0;//We use Y-splitters so motors 5 and 6 are signaled by 1 and 3, but are powered by powerexpander
nMotorEncoder[launcher3] = 0;
startTask(calcRPM);//start RPM calculation
float gain = 0.00;//.003//gain for integral causes aggressive recovery of RPM after firing.
bool running = false;
SensorValue[flywheel] = 0;//clear flywheel encoder so prevent random data from causing error.
bool firstcross = true;
int prevset = 0;
int del = 75;
while(true)
{
Ferror = fRPM - altRPM;
speed = speed + ((float)Ferror*gain);
if(fRPM ==0)//broken code to prevent overshoot, will be removed later.
{
fSpeedControl(0);
speed = motor[launcher1];
}
else if((Ferror>0)!= (fpreverror>0)||Ferror<-200)//if the error has changed signs (positive to negative)
{// or the error is way overshooting, run TBH algorithm
if(firstcross)// if it's the first time, use the approximate speed
{
fSpeedControl(speedapprox);
tbh = speedapprox;
//speed = motor[launcher1];//causes errors. to be removed.
firstcross = false;
gain = 0.012;//0.0065
}
else//If
{
speed = .5*(speed+tbh);//basically averages power between a lower and higher power, to get closer
tbh = speed;// to the needed value.
}
}
else
{
fSpeedControl(speedapprox);//if run to approxspeed incase system fails
tbh = speedapprox;
//speed = tbh+1;
fpreverror = Ferror;//set previous error for algorithm above.
}
if(speed > 120)
{
speed = 120;
}
if(speed <0)
{
speed = 0;
}
motor[launcher4] = speed;
motor[launcher3] = speed;//set the speed for the motors
motor[launcher2] = speed;
motor[launcher1] = speed;
//fSpeedControl(speed,3);
writeDebugStreamLine("%d,%d,%4.1f,%3.1f,%3.1d,%d",nSysTime,fRPM,altfilteredRPM,filteredRPM,speed,tbh);
//^prints data to debug stream for grpahing and analysis of the flywheel's performance.
//enables easier tuning and much quicker adjustment, since it logs all flywheel activities.
delay(del);
/* if(prevset!=fRPM)//was commented out to debug, not sure if causes errors
{//kept off to keep working
firstcross = true;//if the set speed changes, use slew rate to get to approx, then use normal
}
else
{//otherwise continue to check for change in set speed
prevset = fRPM;
}*/
if(fRPM == 0)
{
firstcross = true;//resets values for the next set speed to prevent errors.
tbh= 0;
del = 85;
gain = 0.0;
}
else
{
del = 75;
}
fpreverror = Ferror;
}
}
task autonomous()
{
switch(autopick)//We use a switch case statement to choose autonomous for ease of scalability
{ //and because it is easiest to integrate with the LCD picking in driver mode.
default://any extraneous case caused by a bug, goto basic autonomous
autopick = 0;
break;
//\/\/\/\/\/\/
case 0:
shots = 0;
clear();
startTask(TBHFlywheel);//This does not work, as it says the task is always waiting or not running
fRPM = 2660;//get to correct speed using TBH
speedapprox = 78;//set approximate speed for quick firing.
wait1Msec(2000);
autoIntake(100,0,0);
wait1Msec(2000);
autoIntake(100,0,0);
wait1Msec(2000);
autoIntake(100,0,0);
wait1Msec(2200);
autoIntake(100,0,0);
fRPM = 0;//spin down if finished early.
break;
//\/\/\/\/\/\/
case 1:
clear();
stopTask(TBHFlywheel);
stopTask(usercontrol);//it works if I stop both tasks. Maybe stopping Usercontrol will prevent FRPM from being reset.
spinUP(81);//SpinUp manually increases speed using a safe slew rate
wait1Msec(2500);
autoIntake(85,2);
spinUP(84);
wait1Msec(300);
spinUP(81);
wait1Msec(600);
autoIntake(85,0);
spinUP(84);
wait1Msec(300);
spinUP(81);
wait1Msec(600);
autoIntake(85,0);
spinUP(84);
wait1Msec(300);
spinUP(81);
wait1Msec(600);
autoIntake(85,0);
spinUP(0);
startTask(TBHFlywheel);
break;
//\/\/\/\/\/\/
case 2:
break;
//\/\/\/\/\/\/
case 3:
EncoderBase(100,1750);//old autonomous to capture balls.
wait1Msec(100);
EncoderTurn(100,510,right);
wait1Msec(100);
EncoderBase(100,1000);
wait1Msec(100);
EncoderTurn(100,180, right);
wait1Msec(100);
EncoderBase(100,300);
wait1Msec(100);
EncoderTurn(100,180, right);
wait1Msec(100);
motor[intake] = -100;
EncoderBase(100,700);
stopbase();
motor[intake] = 0;
break;
case 6:
break;
case 7:
shots = 0;
clear();
stopTask(usercontrol);
stopTask(TBHFlywheel);
// fRPM = 2300;//get to correct speed
spinUP(67);//get to correct speed
wait1Msec(2000);
while(shots <=30)// run while shots is less than or equal to 30
{
autoIntake(90,0,1);//intake without time out, both rollers
spinUP(69);//recover lost energy in wheel by increasing voltage
wait1Msec(600);//wait for work to finish
spinUP(67);//adjust to correct speed again
wait1Msec(200);//wait for adjust
shots++;//increment shots taken.
}
fRPM = 0;//spin down if finished early.
startTask(TBHFlywheel);
break;
}
}
Hope this helps. It is a bit messy because of the comments I made for the notebook.
if your not too proficient…