void autonomous3Left() {
motor[intake] = 127;
motor[indexer] = 127;
startAutoFlywheel(300, HIGH_SPEED_HOLD, LOW_SPEED_HOLD, WAIT_HOLD);
setWheelSpeed(45);
wait1Msec(1300);
setWheelSpeed(0);
wait1Msec(2000);
autonIndex = true;
autonShoot = false;
autonIntake = true;
startTask(stopFlywheel);
startTask(intakeControl);
turnPID(350);
drivePID(1000);
stopTask(intakeControl);
motor[intake] = 0;
motor[indexer] = 0;
turnPID(230);
motor[intake] = -127;
motor[indexer] = -127;
wait1Msec(1000);
turnPID(400);
wait1Msec(200);
setWheelSpeed(80,100);
wait1Msec(1000);
setWheelSpeed(0)
}
void autonomous2Left () {
autonIndex = true;
autonShoot = true;
autonIntake = true;
startTask(intakeControl);
setWheelSpeed(75,100);
wait1Msec(800);
setWheelSpeed(100,75);
wait1Msec(800);
setWheelSpeed(0);
drivePID(-1000);
}
bool testEncoder () {
playSound(soundException);
clearLCD();
displayLCDCenteredString(0,"LIFT");
displayLCDCenteredString(1,"ROBOT");
delay(1000);
SensorValue[encoderError] = 0;
bool performsWell = true;
//Flywheel
clearLCD();
displayLCDCenteredString(0,"Encoder Test");
int initValue = SensorValue[flywheelEncoder];
startAutoFlywheel(VELOCITY_LONG);
delay(1000);
if(SensorValue[flywheelEncoder]==initValue) {
performsWell = false;
displayLCDCenteredString(1,"Failed");
} else {
displayLCDCenteredString(1,"Passed");
}
startTask(stopFlywheel);
//Drivebase
delay(1000);
clearLCD();
displayLCDCenteredString(0,"Drivebase Test");
int initWheelValues[2];
initWheelValues[0] = nMotorEncoder(leftWheel13);
initWheelValues[1] = nMotorEncoder(rightWheel13);
setWheelSpeed();
delay(2000);
if(initWheelValues[0]==nMotorEncoder(leftWheel13)) {
performsWell = false;
displayLCDCenteredString(1,"Left Failed");
} else if(initWheelValues[1]==nMotorEncoder(rightWheel13)) {
performsWell = false;
displayLCDCenteredString(1,"Right Failed");
} else {
displayLCDCenteredString(1,"Passed");
}
setWheelSpeed(0);
return performsWell;
}
int EmssController::qt_metacall(QMetaObject::Call _c, int _id, void **_a)
{
_id = Controller::qt_metacall(_c, _id, _a);
if (_id < 0)
return _id;
if (_c == QMetaObject::InvokeMetaMethod) {
switch (_id) {
case 0: setWheelSpeed((*reinterpret_cast< int(*)>(_a[1])),(*reinterpret_cast< int(*)>(_a[2]))); break;
default: ;
}
_id -= 1;
}
return _id;
}
void autonomous1 () {
startTask(flywheelVelocity);
startAutoFlywheel(VELOCITY_LONG, HIGH_SPEED_LONG, LOW_SPEED_LONG, WAIT_LONG);
wait1Msec(3000);
startTask(intakeControl);
autonIndex = true;
autonIntake = true;
autonShoot = true;
delay(3000);
startTask(stopFlywheel);
//setWheelSpeed(-127);
//delay(420);
setWheelSpeed(0);
}
void autonomous0Left() {
startTask(autonomousIntake);
setWheelSpeed(100,85);
wait1Msec(800);
setWheelSpeed(55,100);
wait1Msec(800);
setWheelSpeed(50);
wait1Msec(1100);//distance to pipe
setWheelSpeed(0);
delay(800);
drivePID(-270);
delay(200);
turnPID(-270);
stopTask(autonomousIntake);
motor[intake] = 0;
motor[indexer] = 0;
delay(100);
drivePID(1250);
delay(100);
turnPID(-260);
motor[indexer] = -127;
motor[intake] = -127;
drivePID(2000);
}
void autonomous0Right() {
startTask(autonomousIntake);
setWheelSpeed(65,100);
wait1Msec(800);
setWheelSpeed(100,65);
wait1Msec(800);
setWheelSpeed(80);
wait1Msec(1000);//distance to pipe
setWheelSpeed(0);
delay(800)
drivePID(-380);
delay(200);
turnPID(360);
stopTask(autonomousIntake);
motor[intake] = 0;
motor[indexer] = 0;
delay(100);
drivePID(1100);
delay(100);
turnPID(350);
motor[indexer] = -127;
motor[intake] = -127;
drivePID(2000);
}
void logDrive () {
setWheelSpeed(
abs(vexRT(Ch3))*vexRT(Ch3)/127,
(abs(vexRT(Ch2))*vexRT(Ch2)/127)>100?100:abs(vexRT(Ch2))*vexRT(Ch2)/127);
}
void tankDrive () {
int deadbands = 10;
setWheelSpeed(vexRT(Ch3)<deadbands?0:vexRT(Ch3),vexRT(Ch2)<deadbands?0:vexRT(Ch2));
}
