|
|
@ -17,47 +17,98 @@
|
|
|
|
#pragma competitionControl(Competition)
|
|
|
|
#pragma competitionControl(Competition)
|
|
|
|
#include "Vex_Competition_Includes.c"
|
|
|
|
#include "Vex_Competition_Includes.c"
|
|
|
|
|
|
|
|
|
|
|
|
// some variable definitions
|
|
|
|
// Definitions here!
|
|
|
|
#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 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
|
|
|
|
#define STOP 0
|
|
|
|
|
|
|
|
// Defines the value for when a motor is stopped.
|
|
|
|
|
|
|
|
|
|
|
|
#define DEADZONE 10
|
|
|
|
#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
|
|
|
|
#define DRIVE_OFFSET 10
|
|
|
|
/*
|
|
|
|
/* Defines the offset used to correct and curves while the robot is driving straight during the
|
|
|
|
23.4 / (2 * pi * 2.075) * 672.2
|
|
|
|
driveTiles(float numberOfTiles, bool direction) function.
|
|
|
|
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
|
|
|
|
#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
|
|
|
|
// How much the wheels should spin in a 90 degree turn
|
|
|
|
#define POINTS_PER_TURN 320
|
|
|
|
#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()
|
|
|
|
// definitions for driveTiles()
|
|
|
|
#define FORWARD true
|
|
|
|
#define FORWARD true
|
|
|
|
#define REVERSE false
|
|
|
|
#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() { // Stop all driving motors
|
|
|
|
void stopDriving() {
|
|
|
|
motor[driveLB] = STOP;
|
|
|
|
motor[driveLB] = STOP;
|
|
|
|
motor[driveLF] = STOP;
|
|
|
|
motor[driveLF] = STOP;
|
|
|
|
motor[driveRB] = STOP;
|
|
|
|
motor[driveRB] = STOP;
|
|
|
|
motor[driveRF] = 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
|
|
|
|
void clearEnc() { // Reset driving motor encoder values to 0
|
|
|
|
nMotorEncoder[driveRB] = 0;
|
|
|
|
nMotorEncoder[driveRB] = 0;
|
|
|
|
nMotorEncoder[driveLB] = 0;
|
|
|
|
nMotorEncoder[driveLB] = 0;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// Explicit Parameters: None
|
|
|
|
|
|
|
|
// Output: Resets the driving encoders to 0, for use in other autonomous functions.
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void shootBall() {
|
|
|
|
void shootBall() {
|
|
|
|
motor[shoot] = MAX_SPEED;
|
|
|
|
motor[shoot] = MAX_SPEED;
|
|
|
|
wait(1.25); // Shooting takes 1.25 seconds.
|
|
|
|
wait(1.25); // Shooting takes 1.25 seconds
|
|
|
|
// Any unwanted extra movement will be undone by the rubber bands.
|
|
|
|
|
|
|
|
motor[shoot] = STOP;
|
|
|
|
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) {
|
|
|
|
void turntoRight(float turns) {
|
|
|
|
clearEnc();
|
|
|
|
clearEnc();
|
|
|
|
while(turns * POINTS_PER_TURN > nMotorEncoder[driveLB]){
|
|
|
|
while(turns * POINTS_PER_TURN > nMotorEncoder[driveLB]){
|
|
|
@ -68,6 +119,12 @@ void turntoRight(float turns) {
|
|
|
|
}
|
|
|
|
}
|
|
|
|
stopDriving();
|
|
|
|
stopDriving();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// 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 turntoLeft(float turns) {
|
|
|
|
void turntoLeft(float turns) {
|
|
|
|
clearEnc();
|
|
|
|
clearEnc();
|
|
|
|
while(turns * POINTS_PER_TURN > nMotorEncoder[driveRB]){
|
|
|
|
while(turns * POINTS_PER_TURN > nMotorEncoder[driveRB]){
|
|
|
|