task drivePID() {
float lError = 0;
float lPrevError = 0;
float lIntegral = 0;
float lDerivative = 0;
float rError = 0;
float rPrevError = 0;
float rIntegral = 0;
float rDerivative = 0;
while(true) {
lError = driveLSetPt - leftDrive();
rError = driveRSetPt - rightDrive();
lIntegral += lError;
rIntegral += rError;
lDerivative = lError - lPrevError;
rDerivative = rError - rPrevError;
setDriveLeft(1*((drivekp*lError)+(driveki*lIntegral)+(drivekd*lDerivative)));
setDriveRight(1*((drivekp*rError)+(driveki*rIntegral)+(drivekd*rDerivative)));
lPrevError = lError;
rPrevError = rError;
writeDebugStreamLine("drive: %f, %f", lError, rError);
}
}
void driveMotors (int driveSpeed)
{
leftDrive(driveSpeed);
rightDrive(driveSpeed);
}
task usercontrol()
{
int LiftDownSelector=0;
int stillspeed = 15;
int stillspeedLeft = 15;
while (1 == 1)
{
const int downSpeed = 15;
const int feedstill = 2;
rightDrive ( vexRT[Ch2]);
leftDrive ( vexRT[Ch3]);
if(vexRT[Btn7U] == 1)
{
SensorValue[latch] = 1;
}
if(vexRT[Btn7R] == 1)
{
SensorValue[latch] = 0;
}
if(vexRT[Btn7D] == 1)
{
SensorValue[conveyer] = 1;
}
if(vexRT[Btn7L] == 1)
{
SensorValue[conveyer] = 0;
}
if(vexRT[Btn8L] == 1)
{
stillspeed = -10;
stillspeedLeft = 10;
}
if(vexRT[Btn8U] == 1)
{
stillspeed = 15;
stillspeedLeft = 15;
}
if(vexRT[Btn8D] == 1)
{
SensorValue[Claw] = 1;
}
if(vexRT[Btn8R] == 1)
{
SensorValue[Claw] = 0;
}
if(vexRT[Btn5U] == 1)
{
liftMotors(127);
}
else if(vexRT[Btn5D] == 1)
{
liftMotors(-127);
}
else
{
if(SensorValue[potLeft] > 1700)
{
motor[leftArmTop] = stillspeedLeft;
motor[leftArmBottom] = stillspeedLeft;
motor[rightArmTop] = stillspeed;
motor[rightArmBottom] = stillspeed;
}
else
{
liftMotors(-downSpeed);
}
}
if(vexRT[Btn6U] == 1)
{
motor[rightFeed] = 127;
motor[leftFeed] = 127;
}
else if(vexRT[Btn6D] == 1)
{
motor[rightFeed] = -127;
motor[leftFeed] = -127;
}
else
{
motor[rightFeed] = feedstill;
motor[leftFeed] = feedstill;
}
if (vexRT[Btn8L]==1)
{
LiftDownSelector=1;
}
// wait1Msec(25); //don't hog the cpu
}
}
void DriveLoop(int rightDirection, int leftDirection, int Target, float kP, float kI, float kD, float max, float min)
{
int rightlasterror, righterror, rightintegral, rightdrive;
int leftlasterror, lefterror, leftintegral, leftdrive;
int lastdifference, difference;
int errorwait = 0;
SensorValue(rightQuad) = 0;
SensorValue(leftQuad) = 0;
while(errorwait == 0)
{
rightlasterror = righterror;
righterror = (Target - abs(SensorValue[rightQuad]));
rightintegral = rightintegral + righterror;
leftlasterror = lefterror;
lefterror = (Target - abs(SensorValue[leftQuad]));
leftintegral = leftintegral + lefterror;
lastdifference = difference;
difference = (abs(SensorValue[leftQuad]) - abs(SensorValue[rightQuad]));
rightdrive = (kP*righterror + kI*rightintegral + kD*rightlasterror);
leftdrive = (kP*lefterror + kI*leftintegral + kD*leftlasterror);
if(rightdrive > max)
{
rightdrive = max;
}
if(rightdrive < -max)
{
rightdrive = -max;
}
if(rightdrive > 0 && rightdrive < min)
{
rightdrive = min;
}
if(rightdrive < 0 && rightdrive > -min)
{
rightdrive = -min;
}
if(leftdrive > max)
{
leftdrive = max;
}
if(leftdrive < -max)
{
leftdrive = -max;
}
if(leftdrive > 0 && leftdrive < min)
{
leftdrive = min;
}
if(leftdrive < 0 && leftdrive > -min)
{
leftdrive = -min;
}
leftDrive(leftDirection*leftdrive);
rightDrive(rightDirection*(rightdrive + 2.45*difference));
if(abs(righterror) > 5 || abs(lefterror) > 5)
{
clearTimer(T1);
}
if(time1(T1) > 50)
{
errorwait = 1;
}
}
driveMotors(0);
}
