Пример #1
0
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);
	}
}
Пример #2
0
//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);


	}
}