You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
148 lines
4.2 KiB
C
148 lines
4.2 KiB
C
#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, port4, driveLF, tmotorVex393_MC29, openLoop, reversed)
|
|
#pragma config(Motor, port5, driveRB, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_2)
|
|
#pragma config(Motor, port6, driveRF, tmotorVex393_MC29, openLoop)
|
|
#pragma config(Motor, port7, bintake, tmotorVex393_MC29, openLoop, reversed)
|
|
#pragma config(Motor, port8, , tmotorVex393_MC29, openLoop)
|
|
#pragma config(Motor, port9, , tmotorVex393_MC29, openLoop)
|
|
#pragma config(Motor, port10, , tmotorVex393_HBridge, openLoop)
|
|
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
|
|
|
|
#pragma platform(VEX2)
|
|
#pragma competitionControl(Competition)
|
|
#include "Vex_Competition_Includes.c"
|
|
|
|
// some variable definitions
|
|
#define MAX_SPEED 127
|
|
#define STOP 0
|
|
#define DEADZONE 10
|
|
|
|
/*
|
|
23.4 / (2 * pi * 2.075) * 360
|
|
23.4 = inches in a tile
|
|
2.075 = exact radius of 4" omniwheels
|
|
2 * pi * r is circumference of the wheel
|
|
360 degrees in a circle
|
|
final calculation is the amount of degrees of rotation per tile of movement
|
|
*/
|
|
#define TILE 646
|
|
|
|
void shootBall() {
|
|
motor[shoot] = MAX_SPEED;
|
|
wait(1.25); // Shooting takes 1.25 seconds.
|
|
motor[shoot] = STOP;
|
|
}
|
|
|
|
void joystickDrive() {
|
|
if(abs(vexRT[Ch3]) > DEADZONE) {
|
|
motor[driveLB] = vexRT[Ch3];
|
|
motor[driveLF] = vexRT[Ch3];
|
|
}
|
|
else {
|
|
motor[driveLB] = STOP;
|
|
motor[driveLF] = STOP;
|
|
}
|
|
if(abs(vexRT[Ch2]) > DEADZONE) {
|
|
motor[driveRB] = vexRT[Ch2];
|
|
motor[driveRF] = vexRT[Ch2];
|
|
}
|
|
else {
|
|
motor[driveRB] = STOP;
|
|
motor[driveRF] = STOP;
|
|
}
|
|
}
|
|
|
|
void buttonChecks() {
|
|
if (vexRT[Btn5U] == 1) {
|
|
motor[bintake] = MAX_SPEED;
|
|
}
|
|
else if (vexRT[Btn5D] == 1) {
|
|
motor[bintake] = -MAX_SPEED;
|
|
}
|
|
else {
|
|
motor[bintake] = STOP;
|
|
}
|
|
//Flipper Code
|
|
if (vexRT[Btn8D] == 1) {
|
|
shootBall();
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void pre_auton() {
|
|
/* Set bStopTasksBetweenModes to false if you want to keep user created tasks
|
|
running between Autonomous and Driver controlled modes. You will need to
|
|
manage all user created tasks if set to false. */
|
|
bStopTasksBetweenModes = true;
|
|
}
|
|
|
|
void driveTiles(int numberOfTiles, boolean direction) {
|
|
|
|
while(direction && numberOfTiles * TILE > nMotorEncoder[1]) [
|
|
if(abs(nMotorEncoder[1]) - 10 > nMotorEncoder[2]) {
|
|
motor[driveLB] = 100;
|
|
motor[driveLF] = 100;
|
|
motor[driveRB] = 90;
|
|
motor[driveRF] = 90;
|
|
}
|
|
if(abs(nMotorEncoder[2]) - 10 > nMotorEncoder[1]) {
|
|
motor[driveLB] = 90;
|
|
motor[driveLF] = 90;
|
|
motor[driveRB] = 100;
|
|
motor[driveRF] = 100;
|
|
} else {
|
|
motor[driveLB] = 100;
|
|
motor[driveLF] = 100;
|
|
motor[driveRB] = 100;
|
|
motor[driveRF] = 100;
|
|
}
|
|
}
|
|
while(!direction && numberOfTiles * TILE > nMotorEncoder[1]) [
|
|
if(abs(nMotorEncoder[1]) - 10 > nMotorEncoder[2]) {
|
|
motor[driveLB] = -100;
|
|
motor[driveLF] = -100;
|
|
motor[driveRB] = -90;
|
|
motor[driveRF] = -90;
|
|
}
|
|
if(abs(nMotorEncoder[2]) - 10 > nMotorEncoder[1]) {
|
|
motor[driveLB] = -90;
|
|
motor[driveLF] = -90;
|
|
motor[driveRB] = -100;
|
|
motor[driveRF] = -100;
|
|
} else {
|
|
motor[driveLB] = -100;
|
|
motor[driveLF] = -100;
|
|
motor[driveRB] = -100;
|
|
motor[driveRF] = -100;
|
|
}
|
|
}
|
|
}
|
|
task autonomous() {
|
|
shootBall();
|
|
driveTiles(2, true);
|
|
//driveTiles(1, false);
|
|
}
|
|
void turnRight(int turns) {
|
|
while(turns * 360 < nMotorEncoder[1]
|
|
}
|
|
void turnLeft(int turns) {
|
|
|
|
}
|
|
|
|
void clearEnc() {
|
|
nMotorEncoder[1] = 0;
|
|
nMotorEncoder[2] = 0;
|
|
}
|
|
task usercontrol() {
|
|
while (true) {
|
|
//Dual Driving
|
|
joystickDrive();
|
|
buttonChecks();
|
|
}
|
|
} |