task flw_tsk_FeedForwardCntrl(){
pidReset(_fly.flyPID);
pidInit(_fly.flyPID, 0.6, 0.05, 0, 0, 999999);
int integralLimit;
if(_fly.flyPID.kI == 0){
integralLimit = 0;
} else{
integralLimit = 13.0 / _fly.flyPID.kI;
}
float output = 0;
float initTime = nPgmTime;
while(true){
fw_ButtonControl();
//
//we do not want to zero our error sum when we cross
if(abs(_fly.flyPID.errorSum) > integralLimit){
_fly.flyPID.errorSum = integralLimit * _fly.flyPID.errorSum/(abs(_fly.flyPID.errorSum));
}
_fly.currSpeed = FwCalculateSpeed();
float outVal = pidExecute(_fly.flyPID, _fly.setPoint - _fly.currSpeed);
float dTime = nPgmTime - initTime;
initTime = nPgmTime;
playTone(_fly.currSpeed/2,2);
output = _fly.pred + outVal;
if(output < 0){
output = 0;
}
if(output > 127){
output = 127;
}
if((_fly.currSpeed <= 50 && _fly.setPoint == 0) && vexRT[Btn7L] == 1){
output = -90;
_updateFlyWheel(output);
wait1Msec(4000);
_updateFlyWheel(0);
wait1Msec(10000);
}
_updateFlyWheel(output);
delay(FW_LOOP_SPEED);
}
}
//Gyro turn to target angle
void gyroTurn(float target)
{
if(abs(target) < 40)
pidInit(gyroPid, 3, 0, 0.15, 3, 1270);
bool atGyro = false;
long atTargetTime = nPgmTime;
long timer = nPgmTime;
pidReset(gyroPid);
gyroResetAngle();
while(!atGyro)
{
//Calculate the delta time from the last iteration of the loop
float dT = (float)(nPgmTime - timer)/1000;
//Calculate the current angle of the gyro
float angle = gyroAddAngle(dT);
//Reset loop timer
timer = nPgmTime;
//Calculate the output of the PID controller and output to drive motors
float error = (float)target - angle;
float driveOut = pidExecute(gyroPid, error);
driveL(-driveOut);
driveR(driveOut);
//Stop the turn function when the angle has been within 3 degrees of the desired angle for 350ms
if(abs(error) > 3)
atTargetTime = nPgmTime;
if(nPgmTime - atTargetTime > 350)
{
atGyro = true;
driveL(0);
driveR(0);
}
}
//Reinitialize the PID constants to their original values in case they were changed
pidInit(gyroPid, 2, 0, 0.15, 2, 1270);
}
task autonomous()
{
clearTimer(T1);
pidInit(flywheel, 0.01, 0.03, 0.001, 3, 50);//p, i, d, epsilon, slew
unsigned long lastTime = nPgmTime;
resetMotorEncoder(flywheelLeftBot);
float rpm, lastRpm1, lastRpm2, lastRpm3, lastRpm4;
int counter = 0;
bool btn7DPressed;
while (true)
{
if(1==1)
{
dT = (float)(nPgmTime - lastTime)/1000;
lastTime = nPgmTime;
if(dT != 0)
{
rpm = 60.00*25*(((float)getMotorEncoder(flywheelLeftBot))/dT)/360;
}
else
{
rpm = 0;
}
resetMotorEncoder(flywheelLeftBot);
lastRpm4 = lastRpm3;
lastRpm3 = lastRpm2;
lastRpm2 = lastRpm1;
lastRpm1 = rpm;
rpmAvg = (rpm + lastRpm1 + lastRpm2 + lastRpm3 + lastRpm4) / 5;
if(flywheel.errorSum > 18000)
{
flywheel.errorSum = 18000;
}
if(rpmAvg >= sp && !set)
{
set = true;
}
if( rpm <= sp - 700)
{
set = false;
}
out = pidExecute(flywheel, sp-rpmAvg);
if(vexRT(Btn7U) == 1)
{
btn7DPressed = false;
pidReset(flywheel);
out = 0;
}
if(out < 1)
{
out = 1;
}
driveFlywheel(out);
}
// debugStrea
if(time1[T1] >= 7250 && time1[T1] <= 9000)
{
motor[conveyor] = 127;
motor[conveyor2] = 127;
}
}
/*dank memes
*/
/*
motor[flywheelLeftTop] = 63;
motor[flywheelLeftBot] = 63;
motor[flywheelRightTop] = 63;
motor[flywheelRightBot] = 63;
wait1Msec(2500);
motor[conveyor] = 127;
wait1Msec(1000);
motor[conveyor] = 0;
wait1Msec(1500);
motor[conveyor] = 127;
wait1Msec(1000);
motor[conveyor] = 0;
wait1Msec(1000);
motor[flywheelLeftTop] = 65;
motor[flywheelLeftBot] = 65;
motor[flywheelRightTop] = 65;
motor[flywheelRightBot] = 65;
motor[conveyor] = 127;
wait1Msec(1000);
motor[conveyor] = 0;
wait1Msec(1000);
motor[conveyor] = 127;
wait1Msec(1000);
motor[conveyor] = 0;
*/
//PROGRAMMING SKILLS
/* while(1==1)
{
motor[flywheelLeftTop] = 63;
motor[flywheelLeftBot] = 63;
motor[flywheelRightTop] = 63;
motor[flywheelRightBot] = 63;
motor[conveyor] = 127;
}*/
}
task usercontrol()
{
pidReset(flywheel);
debugStreamClear;
float sp = 2050;
// User control code here, inside the loop
int currentFlywheelSpeed;
// pidInit(flywheel, 0.10, 0.01, 0.00001, 3, 50);
pidInit(flywheel, 0.01, 0.03, 0.001, 3, 50);
unsigned long lastTime = nPgmTime;
resetMotorEncoder(flywheelLeftBot);
float rpm, lastRpm1, lastRpm2, lastRpm3, lastRpm4;
int counter = 0;
bool btn7DPressed;
while (true)
{
if(vexRT[btn7D] == 1)
{
btn7DPressed = true;
}
if(btn7DPressed == true)
{
dT = (float)(nPgmTime - lastTime)/1000;
lastTime = nPgmTime;
if(dT != 0)
{
rpm = 60.00*25*(((float)getMotorEncoder(flywheelLeftBot))/dT)/360;
}
else
{
rpm = 0;
}
resetMotorEncoder(flywheelLeftBot);
lastRpm4 = lastRpm3;
lastRpm3 = lastRpm2;
lastRpm2 = lastRpm1;
lastRpm1 = rpm;
rpmAvg = (rpm + lastRpm1 + lastRpm2 + lastRpm3 + lastRpm4) / 5;
if(flywheel.errorSum > 18000)
{
flywheel.errorSum = 18000;
}
if(rpmAvg >= sp && !set)
{
set = true;
}
if( rpm <= sp - 700)
{
set = false;
}
out = pidExecute(flywheel, sp-rpmAvg);
if(vexRT(Btn7U) == 1)
{
btn7DPressed = false;
pidReset(flywheel);
out = 0;
}
if(out < 1)
{
out = 1;
}
driveFlywheel(out);
}
/*
//motor[Motor1] = 127;
motor[flywheelLeftTop] = 68;
motor[flywheelLeftBot] = 68;
motor[flywheelRightTop] = 68;
motor[flywheelRightBot] = 68;
currentFlywheelSpeed = 68;
}
if(vexRT[Btn7L] == 1)
{
currentFlywheelSpeed -= 2;
motor[flywheelLeftTop] = currentFlywheelSpeed;
motor[flywheelLeftBot] = currentFlywheelSpeed;
motor[flywheelRightTop] = currentFlywheelSpeed;
motor[flywheelRightBot] = currentFlywheelSpeed;
wait1Msec(400);
}
if(vexRT[Btn7R] == 1)
{
currentFlywheelSpeed += 2;
motor[flywheelLeftTop] = currentFlywheelSpeed;
motor[flywheelLeftBot] = currentFlywheelSpeed;
motor[flywheelRightTop] = currentFlywheelSpeed;
motor[flywheelRightBot] = currentFlywheelSpeed;
wait1Msec(400);
}
if(vexRT[Btn7U] == 1)
{
// motor[Motor1] = 0;
motor[flywheelLeftTop] = 0;
motor[flywheelLeftBot] = 0;
motor[flywheelRightTop] = 0;
motor[flywheelRightBot] = 0;
}
if(vexRT[Btn8L] == 1)
{
motor[flywheelLeftTop] = 48;
motor[flywheelLeftBot] = 48;
motor[flywheelRightTop] = 48;
motor[flywheelRightBot] = 48;
currentFlywheelSpeed = 48;
}*/
if(vexRT[Btn8D] == 1)
{
motor[conveyor] = 127;
motor[conveyor2] = 127;
}
if(vexRT[Btn8R] == 1)
{
motor[conveyor] = 0;
motor[conveyor2] = 0;
}
if(vexRT[Btn8U] == 1)
{
motor[conveyor] = -127;
motor[conveyor2] = -127;
}
/* motor[driveFrontRight] = vexRT[Ch1] + vexRT[Ch4];
motor[driveFrontLeft] = vexRT[Ch1] - vexRT[Ch4];
motor[driveBackRight] = vexRT[Ch1] - vexRT[Ch4];
motor[driveBackLeft] = vexRT[Ch1] + vexRT[Ch4];*/
motor[driveFrontRight] = vexRT[Ch2];
motor[driveBackRight] = vexRT[Ch2];
motor[driveFrontLeft] = vexRT[Ch3];
motor[driveBackLeft] = vexRT[Ch3];
while(vexRT(Btn5U)) //Left strafe
{
motor[driveFrontRight] = 127;
motor[driveBackRight] = -127;
motor[driveFrontLeft] = -127;
motor[driveBackLeft] = 127;
}
while(vexRT(Btn6U))//RIGHT STRAFE
{
motor[driveFrontRight] = -127;
motor[driveBackRight] = 127;
motor[driveFrontLeft] = 127;
motor[driveBackLeft] = -127;
}
/*
if(vexRT[Ch2] > 2)
{
motor[driveFrontRight] = 127;
motor[driveFrontLeft] = -127;
motor[driveBackRight] = 127;
motor[driveBackLeft] = -127;
}
if(vexRT[Ch2] < -2)
{
motor[driveFrontRight] = -127;
motor[driveFrontLeft] = 127;
motor[driveBackRight] = -127;
motor[driveBackLeft] = 127;
}
*/
writeDebugStreamLine("%f", rpm);
wait1Msec(20);
}
}