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); }