|
|
#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"
|
|
|
|
|
|
// Definitions here!
|
|
|
|
|
|
#define MAX_SPEED 127
|
|
|
// Set motor maximum speed, this allows for tweaking the speed of the robot with one change.
|
|
|
|
|
|
#define MAX_AUTO_SPEED 100
|
|
|
/* During the development of the autonomous portion of our code, we found that the robot
|
|
|
would have issues turning and driving at MAX_SPEED causing it to turn too much, and
|
|
|
not driving straight. After limiting the driving speed to 100, we found that the robot
|
|
|
was able to drive more consistently.
|
|
|
*/
|
|
|
|
|
|
#define STOP 0
|
|
|
// Defines the value for when a motor is stopped.
|
|
|
|
|
|
#define DEADZONE 10
|
|
|
/* Defines the deadzone of the VEX controller. With our controllers, a value of 10 allowed
|
|
|
for the motors to completely lose power when the joystick is let go.
|
|
|
*/
|
|
|
|
|
|
#define DRIVE_OFFSET 10
|
|
|
/* Defines the offset used to correct curves while the robot is driving straight during the
|
|
|
driveTiles(float numberOfTiles, bool direction) function.
|
|
|
*/
|
|
|
|
|
|
#define TILE 1206
|
|
|
/* Definition for Rotation points per tile.
|
|
|
|
|
|
Each tile is 23.4 inches wide.
|
|
|
|
|
|
|
|
|
The exact radius of 4" omni wheels, using dial calipers: 2.075 inches
|
|
|
|
|
|
2 * pi * r is circumference of the wheel - 13.0376 inches
|
|
|
|
|
|
|
|
|
There are 627.2 points in a revolution with the vex direct motor encoders - according to robotc
|
|
|
developers: “The 2-wire 393 motor measures 627.2 counts per revolution of the output shaft in its
|
|
|
default high-torque configuration and 392 counts per revolution of the output shaft in its
|
|
|
modified high-speed configuration.”
|
|
|
|
|
|
|
|
|
So if we do 1 revolution * distance / radius we get 627.2 * 23.4 / 13.0376 = 1206.
|
|
|
When the integrated motor encoder reports a movement of 1206, that means the robot has moved 1 tile.
|
|
|
*/
|
|
|
|
|
|
|
|
|
// How much the wheels should spin in a 90 degree turn
|
|
|
#define POINTS_PER_TURN 320
|
|
|
/* Using trial and error, we found that our robot will make a 90 degree turn when the integrated motor
|
|
|
encoders report a distance of 320 while spinning in place.
|
|
|
*/
|
|
|
|
|
|
|
|
|
// definitions for driveTiles()
|
|
|
#define FORWARD true
|
|
|
#define REVERSE false
|
|
|
/* When the function driveTiles(float numberOfTiles, bool direction) is called, one of the explicit
|
|
|
parameters is a boolean for direction, where true is forward, and false is reverse. Using these
|
|
|
definitions in our code, it is clearer to us and readers as to what that parameter is for.
|
|
|
*/
|
|
|
|
|
|
void stopDriving() {
|
|
|
motor[driveLB] = STOP;
|
|
|
motor[driveLF] = STOP;
|
|
|
motor[driveRB] = STOP;
|
|
|
motor[driveRF] = STOP;
|
|
|
}
|
|
|
// Explicit Parameters: None
|
|
|
// Output: All four driving motors will be stopped, stopping the robot’s movements immediately.
|
|
|
|
|
|
|
|
|
|
|
|
void clearEnc() { // Reset driving motor encoder values to 0
|
|
|
nMotorEncoder[driveRB] = 0;
|
|
|
nMotorEncoder[driveLB] = 0;
|
|
|
}
|
|
|
// Explicit Parameters: None
|
|
|
// Output: Resets the driving encoders to 0, for use in other autonomous functions.
|
|
|
|
|
|
|
|
|
|
|
|
void shootBall() {
|
|
|
motor[shoot] = MAX_SPEED;
|
|
|
wait(1.25); // Shooting takes 1.25 seconds
|
|
|
motor[shoot] = STOP;
|
|
|
}
|
|
|
// Explicit Parameters: None
|
|
|
// Output: The 2 motors connected to the shoot port will turn on for 1.25 seconds, which is
|
|
|
// precisely the amount of time needed for the motors to pull back and release the launcher.
|
|
|
|
|
|
|
|
|
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();
|
|
|
}
|
|
|
// Explicit Parameters: A floating point number turns will control how much the robot will turn to the right.
|
|
|
// When turns is set to 1, the robot will turn exactly 90 degrees. Since it is a floating point number, we can
|
|
|
// specify decimal amounts to turns to allow for any angle of a turn.
|
|
|
// Output: The robot will turn by (turns * 90) degrees to the right.
|
|
|
|
|
|
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();
|
|
|
}
|
|
|
// Explicit Parameters: A floating point number turns will control how much the robot will turn to the left.
|
|
|
// When turns is set to 1, the robot will turn exactly 90 degrees. Since it is a floating point number, we can
|
|
|
// specify decimal amounts to turns to allow for any angle of a turn.
|
|
|
// Output: The robot will turn by (turns * 90) degrees to the left.
|
|
|
|
|
|
|
|
|
|
|
|
void flipOn() {
|
|
|
motor[bintake] = -MAX_SPEED;
|
|
|
}
|
|
|
void ballOff() {
|
|
|
motor[bintake] = STOP;
|
|
|
}
|
|
|
void ballIn() {
|
|
|
motor[bintake] = MAX_SPEED;
|
|
|
}
|
|
|
// Explicit Parameters: None
|
|
|
// Output: These three functions manage the ball lift and, conveniently, the same motors in reverse will
|
|
|
// flip a cap. flipOn() will spin the motors in the direction needed to flip caps, ballIn() will spin the
|
|
|
// motors in the direction needed to collect and pick up balls, and ballOff() will turn off the motors.
|
|
|
|
|
|
|
|
|
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;
|
|
|
}
|
|
|
}
|
|
|
// Explicit Parameters: None
|
|
|
// Output: The robot will drive based on the values read from the 2 joysticks on the controller. However,
|
|
|
// if the joystick’s value is inside the DEADZONE (10) then the robot will not move. This prevents wasted
|
|
|
// battery and motor overheating when the robot is not supposed to be moving. This is necessary because when
|
|
|
// the joysticks are let go they don’t read a value of exactly zero, it’s usually off by a few.
|
|
|
|
|
|
|
|
|
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!
|
|
|
}
|
|
|
// Explicit Parameters: None
|
|
|
// Output: When the corresponding buttons are pressed, various features will be activated, such as the cap
|
|
|
// flipper or the ball launcher. When the buttons are released, the action is stopped.
|
|
|
|
|
|
|
|
|
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;
|
|
|
}
|
|
|
// Auto-generated ROBOTC autonomous function
|
|
|
|
|
|
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();
|
|
|
}
|
|
|
// Explicit Parameters: A floating point number numberOfTurns that represents the number of tiles that the
|
|
|
// robot is to drive. Since it is a floating point number, we can move by half or any fraction movement.
|
|
|
// There is also the boolean value direction that controls which way the robot is to move. true is for forward,
|
|
|
// and false is for reverse.
|
|
|
// Output: The robot will drive the specified amount of tiles, in the specified direction. If the robot is not driving
|
|
|
// straight, the speeds of the left and right motors can be offset from each other to cancel out any slight drifts
|
|
|
// to the left or right. We subtract 200 from the distance no matter what here, because the robot moves
|
|
|
// that much after it is told to stop.
|
|
|
|
|
|
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(); // So we can pick up the ball that's under it!
|
|
|
driveTiles(0.5, FORWARD);
|
|
|
ballIn();
|
|
|
driveTiles(0.1, REVERSE);
|
|
|
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);
|
|
|
}
|
|
|
/*
|
|
|
This is the autonomous task. Here’s the path of the robot, described in words instead of code:
|
|
|
|
|
|
1. Start at the red tile closest to the flags.
|
|
|
|
|
|
2. Turn a tiny bit to the right to aim, and shoot the top flag with our preload.
|
|
|
|
|
|
3. Re-align ourselves and drive to hit the bottom flag.
|
|
|
|
|
|
4. Back up 1 tile, turn right, and back into the wall to align the robot.
|
|
|
|
|
|
5. Drive forward with the flipper turned on, and flip the cap from blue to red.
|
|
|
|
|
|
6. Back up, turn left, reverse 1 tile, turn right, back into the wall again to align ourselves.
|
|
|
|
|
|
7. Drive forward, push the cap off of the ball, turn of the flipper and flip the cap.
|
|
|
|
|
|
8. Run the motors to lift the ball up to the ball launcher. We wait a few seconds for the ball.
|
|
|
|
|
|
9. Turn to the left, wait a bit more, and shoot the top flag in the middle column of flags.
|
|
|
|
|
|
10. Turn right, back up to the starting tile, then turn left.
|
|
|
|
|
|
11. Reverse for 1 tile to be perpendicular with the platforms.
|
|
|
|
|
|
12. Turn to face the parking platforms, and reverse into the wall to align ourselves again.
|
|
|
|
|
|
13. Climb to the middle parking platform and stop.
|
|
|
*/
|
|
|
|
|
|
|
|
|
task usercontrol() {
|
|
|
while (true) {
|
|
|
joystickDrive();
|
|
|
buttonChecks();
|
|
|
}
|
|
|
}
|
|
|
// When the driver is in control, this task runs. For the entire duration of the driver control period, we need
|
|
|
// to be able to control the robot, so we put everything in a while loop. The task calls 2 previously mentioned
|
|
|
// functions, joystickDrive() and buttonChecks().
|
|
|
|