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.
227 lines
7.0 KiB
C
227 lines
7.0 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_2)
|
|
#pragma config(Motor, port4, driveLF, tmotorVex393_MC29, openLoop, reversed)
|
|
#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)
|
|
#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 // Max speed of the motors
|
|
#define MAX_AUTO_SPEED 100 // Max speed of the driving motors during the autonomous code
|
|
#define STOP 0
|
|
#define DEADZONE 10
|
|
#define DRIVE_OFFSET 10
|
|
/*
|
|
23.4 / (2 * pi * 2.075) * 672.2
|
|
23.4 = inches in a tile
|
|
2.075 = exact radius of 4" omniwheels
|
|
2 * pi * r is circumference of the wheel
|
|
627.2 points in a revolution with the vex encoders
|
|
final calculation is the amount of points of rotation per tile of movement
|
|
*/
|
|
#define TILE 1206
|
|
|
|
// How much the wheels should spin in a 90 degree turn
|
|
#define POINTS_PER_TURN 320
|
|
|
|
// definitions for driveTiles()
|
|
#define FORWARD true
|
|
#define REVERSE false
|
|
|
|
void stopDriving() { // Stop all driving motors
|
|
motor[driveLB] = STOP;
|
|
motor[driveLF] = STOP;
|
|
motor[driveRB] = STOP;
|
|
motor[driveRF] = STOP;
|
|
}
|
|
|
|
void clearEnc() { // Reset driving motor encoder values to 0
|
|
nMotorEncoder[driveRB] = 0;
|
|
nMotorEncoder[driveLB] = 0;
|
|
}
|
|
|
|
void shootBall() {
|
|
motor[shoot] = MAX_SPEED;
|
|
wait(1.25); // Shooting takes 1.25 seconds.
|
|
// Any unwanted extra movement will be undone by the rubber bands.
|
|
motor[shoot] = STOP;
|
|
}
|
|
void turntoRight(float turns) {
|
|
clearEnc();
|
|
while(turns * POINTS_PER_TURN > nMotorEncoder[driveLB]){
|
|
motor[driveLB] = MAX_AUTO_SPEED;
|
|
motor[driveLF] = MAX_AUTO_SPEED;
|
|
motor[driveRB] = -MAX_AUTO_SPEED;
|
|
motor[driveRF] = -MAX_AUTO_SPEED;
|
|
}
|
|
stopDriving();
|
|
}
|
|
void turntoLeft(float turns) {
|
|
clearEnc();
|
|
while(turns * POINTS_PER_TURN > nMotorEncoder[driveRB]){
|
|
motor[driveLB] = -MAX_AUTO_SPEED;
|
|
motor[driveLF] = -MAX_AUTO_SPEED;
|
|
motor[driveRB] = MAX_AUTO_SPEED;
|
|
motor[driveRF] = MAX_AUTO_SPEED;
|
|
}
|
|
stopDriving();
|
|
}
|
|
|
|
|
|
void flipOn() {
|
|
motor[bintake] = -MAX_SPEED;
|
|
}
|
|
void ballOff() {
|
|
motor[bintake] = STOP;
|
|
}
|
|
void ballIn() {
|
|
motor[bintake] = MAX_SPEED;
|
|
}
|
|
|
|
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) {
|
|
ballIn();
|
|
}
|
|
else if (vexRT[Btn5D] == 1) {
|
|
flipOn();
|
|
}
|
|
else {
|
|
ballOff();
|
|
}
|
|
if (vexRT[Btn8D] == 1) {
|
|
shootBall();
|
|
} // No need for reverse on the ball launcher!
|
|
}
|
|
|
|
|
|
|
|
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(float numberOfTiles, bool direction) {
|
|
// when direction is true, move forward, otherwise go in reverse
|
|
clearEnc();
|
|
while(direction == FORWARD && numberOfTiles * TILE - 200 > nMotorEncoder[driveRB]) {
|
|
if(abs(nMotorEncoder[driveRB]) - DRIVE_OFFSET > nMotorEncoder[driveLB]) {
|
|
motor[driveLB] = MAX_AUTO_SPEED;
|
|
motor[driveLF] = MAX_AUTO_SPEED;
|
|
motor[driveRB] = MAX_AUTO_SPEED - DRIVE_OFFSET;
|
|
motor[driveRF] = MAX_AUTO_SPEED - DRIVE_OFFSET;
|
|
}
|
|
if(abs(nMotorEncoder[driveLB]) - DRIVE_OFFSET > nMotorEncoder[driveRB]) {
|
|
motor[driveLB] = MAX_AUTO_SPEED - DRIVE_OFFSET;
|
|
motor[driveLF] = MAX_AUTO_SPEED - DRIVE_OFFSET;
|
|
motor[driveRB] = MAX_AUTO_SPEED;
|
|
motor[driveRF] = MAX_AUTO_SPEED;
|
|
} else {
|
|
motor[driveLB] = MAX_AUTO_SPEED;
|
|
motor[driveLF] = MAX_AUTO_SPEED;
|
|
motor[driveRB] = MAX_AUTO_SPEED;
|
|
motor[driveRF] = MAX_AUTO_SPEED;
|
|
}
|
|
}
|
|
while(direction == REVERSE && numberOfTiles * TILE - 200 > -nMotorEncoder[driveRB]) {
|
|
if(abs(nMotorEncoder[driveRB]) - DRIVE_OFFSET > nMotorEncoder[driveLB]) {
|
|
motor[driveLB] = -MAX_AUTO_SPEED;
|
|
motor[driveLF] = -MAX_AUTO_SPEED;
|
|
motor[driveRB] = -MAX_AUTO_SPEED + DRIVE_OFFSET;
|
|
motor[driveRF] = -MAX_AUTO_SPEED + DRIVE_OFFSET;
|
|
}
|
|
if(abs(nMotorEncoder[driveLB]) - DRIVE_OFFSET > nMotorEncoder[driveLB]) {
|
|
motor[driveLB] = -MAX_AUTO_SPEED + DRIVE_OFFSET;
|
|
motor[driveLF] = -MAX_AUTO_SPEED + DRIVE_OFFSET;
|
|
motor[driveRB] = -MAX_AUTO_SPEED;
|
|
motor[driveRF] = -MAX_AUTO_SPEED;
|
|
} else {
|
|
motor[driveLB] = -MAX_AUTO_SPEED;
|
|
motor[driveLF] = -MAX_AUTO_SPEED;
|
|
motor[driveRB] = -MAX_AUTO_SPEED;
|
|
motor[driveRF] = -MAX_AUTO_SPEED;
|
|
}
|
|
}
|
|
stopDriving();
|
|
}
|
|
|
|
|
|
task autonomous() {
|
|
turntoRight(0.03);
|
|
shootBall();
|
|
turntoLeft(0.03);
|
|
driveTiles(2, FORWARD); // Move 2 forward to hit bottom flag
|
|
driveTiles(1, REVERSE);
|
|
turntoRight(1);
|
|
driveTiles(0.5, REVERSE); // Drive 1/3 of a tile backwards to hit the wall and align ourselves!
|
|
flipOn(); // Turn on the ball intake in reverse, which is what we can use to flip the caps
|
|
driveTiles(1.6, FORWARD); // flip cap
|
|
ballOff();
|
|
driveTiles(1.35, REVERSE);
|
|
turntoLeft(1);
|
|
driveTiles(1, REVERSE);
|
|
turntoRight(1);
|
|
driveTiles(0.6, REVERSE);
|
|
driveTiles(2.1, FORWARD); // Flip the other cap without turning on the spinner
|
|
flipOn();
|
|
driveTiles(0.5, FORWARD);
|
|
ballIn();
|
|
driveTiles(0.1, REVERSE);
|
|
// So we can pick up the ball!
|
|
wait(3);
|
|
driveTiles(0.1, REVERSE);
|
|
turntoLeft(1);
|
|
driveTiles(0.2, REVERSE);
|
|
wait(3);
|
|
ballOff();
|
|
shootBall();
|
|
driveTiles(0.05, REVERSE);
|
|
driveTiles(0.33, FORWARD);
|
|
wait(2);
|
|
turntoRight(1);
|
|
driveTiles(2.2, REVERSE);
|
|
turntoLeft(1);
|
|
driveTiles(1, REVERSE);
|
|
turntoRight(1);
|
|
driveTiles(0.25, REVERSE);
|
|
driveTiles(3, FORWARD);
|
|
|
|
}
|
|
task usercontrol() { // In user control mode
|
|
while (true) {
|
|
joystickDrive(); // Joystick mapping function
|
|
buttonChecks(); // Button mapping, for lift, ball launcher, etc.
|
|
}
|
|
} |