task driveStraightTask()
{
driveStraightRunning = true;
int coeff = 15;
int totalClicks = 0;
int slavePower = drivePower - 2;
int error = 0;
SensorValue[leftEncoder] = 0;
SensorValue[rightEncoder] = 0;
SensorValue[gyro] = 0;
clearTimer(driveTimer);
while (abs(totalClicks) < clicks && time1(driveTimer) < timeout)
{
setDrivePower(drivePower * direction, slavePower * direction);
error = SensorValue[gyro];
slavePower += error / coeff;
totalClicks += (abs(SensorValue[leftEncoder]) + abs(SensorValue[rightEncoder])) / 2;
SensorValue[leftEncoder] = 0;
SensorValue[rightEncoder] = 0;
wait1Msec(100);
}
setDrivePower(0, 0);
wait1Msec(delayAtEnd);
driveStraightRunning = false;
}
//autonomous region
task turnTask() {
SensorValue[gyro] = 0; //clear the gyro
//turn
setDrivePower(-sgn(degreesToTurn) * maxTurnSpeed, sgn(degreesToTurn) * maxTurnSpeed);
while (abs(SensorValue[gyro]) < abs(degreesToTurn * 10)) {}
//brake
setDrivePower(sgn(degreesToTurn) * 10, -sgn(degreesToTurn) * 10);
int brakeDelay = limit(250, 0, waitAtEnd);
wait1Msec(brakeDelay);
setDrivePower(0, 0);
if (waitAtEnd > 250) wait1Msec(waitAtEnd - 250); //wait at end
}
void driveStraight(int _clicks_, int _leftDirection_, int _rightDirection_, int _drivePower_, bool startAsTask=false, int _delayAtEnd_=250, int _timeout_=15000) {
clicks = _clicks_;
rightDirection = _rightDirection_;
leftDirection = _leftDirection_;
drivePower = _drivePower_;
delayAtEnd = _delayAtEnd_;
timeout = _timeout_;
if (startAsTask) {
startTask(driveStraightTask);
}
else { //runs as function
int coeff = 5;
totalClicks = 0;
int slavePower = drivePower;
int error = 0;
SensorValue[leftEncoder] = 0;
SensorValue[rightEncoder] = 0;
SensorValue[gyro] = 0;
clearTimer(driveTimer);
while (abs(totalClicks) < clicks && time1(driveTimer) < timeout)
{
setDrivePower(drivePower * leftDirection, slavePower * rightDirection);
error = SensorValue[gyro];
slavePower += error / coeff;
totalClicks += (abs(SensorValue[leftEncoder]) + (SensorValue[rightEncoder])) / 2;
SensorValue[leftEncoder] = 0;
SensorValue[rightEncoder] = 0;
SensorValue[gyro] = 0;
wait1Msec(100);
}
setDrivePower(0, 0);
wait1Msec(delayAtEnd);
}
}
void turn(float _degreesToTurn_, int _maxTurnSpeed_=55, bool runAsTask=false, int _waitAtEnd_=250) {
degreesToTurn = _degreesToTurn_;
maxTurnSpeed = _maxTurnSpeed_;
waitAtEnd = _waitAtEnd_;
if (runAsTask) {
startTask(turnTask);
}
else {
SensorValue[gyro] = 0; //clear the gyro
//turn
setDrivePower(sgn(degreesToTurn) * maxTurnSpeed, -sgn(degreesToTurn) * maxTurnSpeed);
while (abs(SensorValue[gyro]) < abs(degreesToTurn * 10)) {}
//brake
setDrivePower(-sgn(degreesToTurn) * 10, sgn(degreesToTurn) * 10);
int brakeDelay = limit(250, 0, waitAtEnd);
wait1Msec(brakeDelay);
setDrivePower(0, 0);
if (waitAtEnd > 250) wait1Msec(waitAtEnd - 250); //wait at end
}
}
void driveRuntime(holonomicDrive *drive) {
int x = vexRT[ drive->xInput ];
int y = vexRT[ drive->yInput ];
if (abs(x) > drive->deadBand || abs(y) > drive->deadBand) {
setDrivePowerByVector(drive, x, y);
}
else {
int turnPower = vexRT[ drive->turnInput ];
setDrivePower(drive, -turnPower, turnPower, false);
}
}
task usercontrol() {
initializeTasks();
setFlywheelRange(0);
while (true)
{
while (vexRT[emergencyStopBtn] == 0)
{
setDrivePower(sgn(vexRT[Ch2]) * vexRT[Ch2] * vexRT[Ch2] / 127, sgn(vexRT[Ch3]) * vexRT[Ch3] * vexRT[Ch3] / 127); //drive
motor[feedMe] = 127*vexRT[feedInBtn] - 127*vexRT[feedOutBtn];
feedMePower = motor[feedMe];
EndTimeSlice();
}
emergencyStop(); //reassign emstop button
}
}
task usercontrol() {
initializeTasks();
setFlywheelRange(0);
while (true)
{
while (vexRT[emergencyStopBtn] == 0)
{
setDrivePower(sgn(vexRT[Ch2]) * vexRT[Ch2] * vexRT[Ch2] / 127, sgn(vexRT[Ch3]) * vexRT[Ch3] * vexRT[Ch3] / 127); //drive
motor[seymore] = abs(targetVelocity - flywheelVelocity) < firingErrorMargin * targetVelocity ? 127*vexRT[fireBtn] - 127*vexRT[seymoreOutBtn] : 0;
motor[feedMe] = 127*vexRT[feedInBtn] - 127*vexRT[feedOutBtn];
EndTimeSlice();
}
emergencyStop(); //reassign emstop button
}
}
void setDrivePowerByVector(holonomicDrive *drive, float x, float y) { //sets motor powers so that drive exerts a force along <x, y> with magnitude proportional to its length
float squareX, squareY
if (x != 0) {
//Input transformed from joystick circle to velocity square. If you want more detail ask Tynan. Sorry.
squareX = sgn(x) / (abs(y/x) + 1);
squareY = squareX * y/x;
}
else {
squareX = 0;
squareY = sgn(y);
}
float magnitude = (x*x + y*y) / 127.0;
setDrivePower(drive, (squareX+squareY)*magnitude, (squareY-squareX)*magnitude);
}
