|
|
|
@ -1,11 +1,11 @@
|
|
|
|
|
#pragma config(I2C_Usage, I2C1, i2cSensors)
|
|
|
|
|
#pragma config(I2C_Usage, I2C1, i2cSensors)
|
|
|
|
|
#pragma config(Sensor, I2C_1, , sensorQuadEncoderOnI2CPort, , AutoAssign )
|
|
|
|
|
#pragma config(Sensor, I2C_2, , sensorQuadEncoderOnI2CPort, , AutoAssign )
|
|
|
|
|
#pragma config(Motor, port1, , tmotorVex393_HBridge, openLoop)
|
|
|
|
|
#pragma config(Motor, port2, shoot, tmotorVex393_MC29, openLoop, reversed)
|
|
|
|
|
#pragma config(Motor, port3, driveLB, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_1)
|
|
|
|
|
#pragma config(Motor, port3, driveLB, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_2)
|
|
|
|
|
#pragma config(Motor, port4, driveLF, tmotorVex393_MC29, openLoop, reversed)
|
|
|
|
|
#pragma config(Motor, port5, driveRB, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_2)
|
|
|
|
|
#pragma config(Motor, port5, driveRB, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_1)
|
|
|
|
|
#pragma config(Motor, port6, driveRF, tmotorVex393_MC29, openLoop)
|
|
|
|
|
#pragma config(Motor, port7, bintake, tmotorVex393_MC29, openLoop, reversed)
|
|
|
|
|
#pragma config(Motor, port8, , tmotorVex393_MC29, openLoop)
|
|
|
|
@ -37,7 +37,7 @@ void shootBall() {
|
|
|
|
|
wait(1.25); // Shooting takes 1.25 seconds.
|
|
|
|
|
motor[shoot] = STOP;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void clearEnc();
|
|
|
|
|
void joystickDrive() {
|
|
|
|
|
if(abs(vexRT[Ch3]) > DEADZONE) {
|
|
|
|
|
motor[driveLB] = vexRT[Ch3];
|
|
|
|
@ -82,46 +82,50 @@ void pre_auton() {
|
|
|
|
|
bStopTasksBetweenModes = true;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void driveTiles(int numberOfTiles, boolean direction) {
|
|
|
|
|
|
|
|
|
|
while(direction && numberOfTiles * TILE > nMotorEncoder[1]) [
|
|
|
|
|
if(abs(nMotorEncoder[1]) - 10 > nMotorEncoder[2]) {
|
|
|
|
|
void driveTiles(int numberOfTiles, bool direction) {
|
|
|
|
|
clearEnc();
|
|
|
|
|
while(direction && numberOfTiles * TILE > nMotorEncoder[1]) {
|
|
|
|
|
if(abs(nMotorEncoder[1]) - 10 > nMotorEncoder[2]) {
|
|
|
|
|
motor[driveLB] = 100;
|
|
|
|
|
motor[driveLF] = 100;
|
|
|
|
|
motor[driveRB] = 90;
|
|
|
|
|
motor[driveRB] = 90;
|
|
|
|
|
motor[driveRF] = 90;
|
|
|
|
|
}
|
|
|
|
|
if(abs(nMotorEncoder[2]) - 10 > nMotorEncoder[1]) {
|
|
|
|
|
if(abs(nMotorEncoder[2]) - 10 > nMotorEncoder[1]) {
|
|
|
|
|
motor[driveLB] = 90;
|
|
|
|
|
motor[driveLF] = 90;
|
|
|
|
|
motor[driveRB] = 100;
|
|
|
|
|
motor[driveRB] = 100;
|
|
|
|
|
motor[driveRF] = 100;
|
|
|
|
|
} else {
|
|
|
|
|
motor[driveLB] = 100;
|
|
|
|
|
motor[driveLB] = 100;
|
|
|
|
|
motor[driveLF] = 100;
|
|
|
|
|
motor[driveRB] = 100;
|
|
|
|
|
motor[driveRB] = 100;
|
|
|
|
|
motor[driveRF] = 100;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
while(!direction && numberOfTiles * TILE > nMotorEncoder[1]) [
|
|
|
|
|
if(abs(nMotorEncoder[1]) - 10 > nMotorEncoder[2]) {
|
|
|
|
|
while(!direction && numberOfTiles * TILE > nMotorEncoder[1]) {
|
|
|
|
|
if(abs(nMotorEncoder[1]) - 10 > nMotorEncoder[2]) {
|
|
|
|
|
motor[driveLB] = -100;
|
|
|
|
|
motor[driveLF] = -100;
|
|
|
|
|
motor[driveRB] = -90;
|
|
|
|
|
motor[driveRB] = -90;
|
|
|
|
|
motor[driveRF] = -90;
|
|
|
|
|
}
|
|
|
|
|
if(abs(nMotorEncoder[2]) - 10 > nMotorEncoder[1]) {
|
|
|
|
|
if(abs(nMotorEncoder[2]) - 10 > nMotorEncoder[1]) {
|
|
|
|
|
motor[driveLB] = -90;
|
|
|
|
|
motor[driveLF] = -90;
|
|
|
|
|
motor[driveRB] = -100;
|
|
|
|
|
motor[driveRB] = -100;
|
|
|
|
|
motor[driveRF] = -100;
|
|
|
|
|
} else {
|
|
|
|
|
motor[driveLB] = -100;
|
|
|
|
|
motor[driveLB] = -100;
|
|
|
|
|
motor[driveLF] = -100;
|
|
|
|
|
motor[driveRB] = -100;
|
|
|
|
|
motor[driveRB] = -100;
|
|
|
|
|
motor[driveRF] = -100;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
motor[driveLB] = 0;
|
|
|
|
|
motor[driveLF] = 0;
|
|
|
|
|
motor[driveRB] = 0;
|
|
|
|
|
motor[driveRF] = 0;
|
|
|
|
|
}
|
|
|
|
|
task autonomous() {
|
|
|
|
|
shootBall();
|
|
|
|
@ -129,10 +133,14 @@ task autonomous() {
|
|
|
|
|
//driveTiles(1, false);
|
|
|
|
|
}
|
|
|
|
|
void turnRight(int turns) {
|
|
|
|
|
while(turns * 360 < nMotorEncoder[1]
|
|
|
|
|
while(turns * 360 < nMotorEncoder[1]){
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
void turnLeft(int turns) {
|
|
|
|
|
while(turns * 360 < nMotorEncoder[1]){
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void clearEnc() {
|
|
|
|
|