#include "Configuration.h"
vex::competition Competition;
void Forward(int s, int n) {
float inch = 28.6478897565;
RD.setStopping(brakeType::coast);
LD.setStopping(brakeType::coast);
FL.resetRotation();
FR.resetRotation();
BL.resetRotation();
BR.resetRotation();
FL.rotateFor(inch * n, rotationUnits::deg, s, velocityUnits::pct, false);
FR.rotateFor(inch * n, rotationUnits::deg, s, velocityUnits::pct, false);
BL.rotateFor(inch * n, rotationUnits::deg, s, velocityUnits::pct, false);
BR.rotateFor(inch * n, rotationUnits::deg, s, velocityUnits::pct, true);
}
void Backward(int s, int n) {
float inch = 28.6478897565;
RD.setStopping(brakeType::coast);
LD.setStopping(brakeType::coast);
FL.resetRotation();
FR.resetRotation();
BL.resetRotation();
BR.resetRotation();
FL.rotateFor(inch * -n, rotationUnits::deg, -s, velocityUnits::pct, false);
FR.rotateFor(inch * -n, rotationUnits::deg, -s, velocityUnits::pct, false);
BL.rotateFor(inch * -n, rotationUnits::deg, -s, velocityUnits::pct, false);
BR.rotateFor(inch * -n, rotationUnits::deg, -s, velocityUnits::pct, true);
}
void TL(int s, int d) {
FL.resetRotation();
FR.resetRotation();
BL.resetRotation();
BR.resetRotation();
FL.rotateFor((d*2)*-1, rotationUnits::deg, -s, velocityUnits::pct, false);
FR.rotateFor((d*2), rotationUnits::deg, s, velocityUnits::pct, false);
BL.rotateFor((d*2)*-1, rotationUnits::deg, -s, velocityUnits::pct, false);
BR.rotateFor((d*2), rotationUnits::deg, s, velocityUnits::pct, true); //45 degree turn on both sides = 90 degree turn total. To halve it halve the amount of degrees on both sides
}
void TR(int s, int d) {
FL.resetRotation();
FR.resetRotation();
BL.resetRotation();
BR.resetRotation();
FL.rotateFor((d*2), rotationUnits::deg, s, velocityUnits::pct, false);
FR.rotateFor((d*2)*-1, rotationUnits::deg, -s, velocityUnits::pct, false);
BL.rotateFor((d*2), rotationUnits::deg, s, velocityUnits::pct, false);
BR.rotateFor((d*2)*-1, rotationUnits::deg, -s, velocityUnits::pct, true); //45 degree turn on both sides = 90 degree turn total. To halve it halve the amount of degrees on both sides
}
void intake(int s, int t){
Intake.spin(directionType::fwd, s, velocityUnits::pct);
task::sleep(t);
Intake.stop(brakeType::hold);
}
void outtake(int s, int t){
Intake.spin(directionType::rev, s, velocityUnits::pct);
task::sleep(t);
Intake.stop(brakeType::hold);
}
void Driving_Intake(int s, int n){
float inch = 28.6478897565;
RD.setStopping(brakeType::coast);
LD.setStopping(brakeType::coast);
FL.resetRotation();
FR.resetRotation();
BL.resetRotation();
BR.resetRotation();
Intake.spin(directionType::fwd, 70, velocityUnits::pct);
task::sleep(500);
FL.rotateFor(inch * n, rotationUnits::deg, s, velocityUnits::pct, false);
FR.rotateFor(inch * n, rotationUnits::deg, s, velocityUnits::pct, false);
BL.rotateFor(inch * n, rotationUnits::deg, s, velocityUnits::pct, false);
BR.rotateFor(inch * n, rotationUnits::deg, s, velocityUnits::pct, true);
Intake.stop(brakeType::hold);
}
void Driving_Outake(int s, int n){
float inch = 28.6478897565;
RD.setStopping(brakeType::coast);
LD.setStopping(brakeType::coast);
FL.resetRotation();
FR.resetRotation();
BL.resetRotation();
BR.resetRotation();
Intake.spin(directionType::rev, 30, velocityUnits::pct);
FL.rotateFor(inch * n, rotationUnits::deg, s, velocityUnits::pct, false);
FR.rotateFor(inch * n, rotationUnits::deg, s, velocityUnits::pct, false);
BL.rotateFor(inch * n, rotationUnits::deg, s, velocityUnits::pct, false);
BR.rotateFor(inch * n, rotationUnits::deg, s, velocityUnits::pct, true);
Intake.stop(brakeType::hold);
}
void expand(){
outtake(100, 900);
task::sleep(1000);
}
void expand_2(){
outtake(100, 900);
task::sleep(1000);
Intake.rotateFor(1500, rotationUnits::deg, 100, velocityUnits::pct, true);
}
void T_Stack(){
/*Intake.spin(directionType::rev, 30, velocityUnits::pct);
task::sleep(600);
Intake.stop(brakeType::hold);*/
outtake(30, 600);
T.rotateTo(-525, rotationUnits::deg, 20, velocityUnits::pct, true);
/*Intake.spin(directionType::rev, 30, velocityUnits::pct);
task::sleep(400);
Intake.stop(brakeType::brake);*/
outtake(30, 400);
Driving_Outake(20, 1);
/*Intake.spin(directionType::rev, 30, velocityUnits::pct);
task::sleep(500);
Backward(30, 8);
Intake.stop(brakeType::hold);*/
outtake(30, 500);
Driving_Outake(-30, -8);
}
void T_Stack_NO(){
Intake.spin(directionType::rev, 30, velocityUnits::pct);
task::sleep(400);
Intake.stop(brakeType::hold);
T.rotateTo(-530, rotationUnits::deg, 20, velocityUnits::pct, true);
Intake.spin(directionType::rev, 30, velocityUnits::pct);
task::sleep(480);
Intake.stop(brakeType::brake);
Intake.spin(directionType::rev, 30, velocityUnits::pct);
task::sleep(500);
Backward(30, 8);
Intake.stop(brakeType::hold);
}
void T_back(){
T.spin(directionType::fwd, 100, velocityUnits::pct);
task::sleep(500);
T.stop(brakeType::hold);
}
int All = 1;
int Red_Select = 0;
int Blue_Select = 0;
int Red_4P = 0;
int Red_PH = 0;
int Blue_4P = 0;
int Blue_PH = 0;
void Alliance() {
Brain.Screen.drawRectangle(0, 0, 240, 240, color::red); // Note that (0,0) is at top left.
// Because (0,0) is on the very
Brain.Screen.printAt(119, 119, "RED"); // first pixel, the POSITION of every pixel is -1 from its actual
Brain.Screen.drawRectangle(240, 0, 240, 240, color::blue); // position. For a 480 x 272 screen,
// the the very bottom right
Brain.Screen.printAt(359, 119,"BLUE"); // pixel is at (479,271), not (480,272).
// In the case of these rectangles, the last 2 numbers are asking for length,
// not position. The first 2 numbers are the position.
if (Brain.Screen.pressing()) {
if (Brain.Screen.xPosition() >= 0 && Brain.Screen.xPosition() <= 239 && Brain.Screen.yPosition() <= 240 && Brain.Screen.yPosition() >= 0) {
task::sleep(500); // gives time to remove any contact with screen
// ^Threshold setting for the left side of the screen.
Red_Select++;
All--; // Red_Select++ and All-- are used to go from one loop to the next.
// "--" Subtracts 1 from integer "All", which makes it 0 and ends
// its loop (0=/1).
} //"++" Adds 1 to "Red_Select", which makes it 1 and start its loop (1==1).
//The same logic applies for the rest.
else if (Brain.Screen.xPosition() >= 240 && Brain.Screen.xPosition() <= 479 && Brain.Screen.yPosition() <= 240 && Brain.Screen.yPosition() >= 0) {
task::sleep(500); //^Threshold setting for the right side of the screen.
Blue_Select++;
All--;
}
else {
task::sleep(0);
}
}
else {
task::sleep(0);
}
}
void Red_Position() {
// Brain.Screen.clearScreen();
Brain.Screen.drawRectangle(0, 0, 240, 240, color::white);
Brain.Screen.printAt(119, 119, "4-Point");
Brain.Screen.drawRectangle(240, 0, 240, 240, color::black);
Brain.Screen.printAt(359, 119, "5-Point");
if (Brain.Screen.pressing()) { // Asks if the brain is being pressed. This stops the
// code from getting the position of the last press.
if (Brain.Screen.xPosition() >= 0 && Brain.Screen.xPosition() <= 239 && Brain.Screen.yPosition() <= 240 && Brain.Screen.yPosition() >= 0) {
task::sleep(500); //^if press was at D:[0<x<239] and R:[0<y<240] pixels, then...
//(Only occurs if screen has been pressed during this void)
Red_4P++;
Red_Select--;
Brain.Screen.clearScreen();
Brain.Screen.setCursor(1, 1);
Brain.Screen.print("Running 4-Point");
}
else if (Brain.Screen.xPosition() >= 240 && Brain.Screen.xPosition() <= 479 && Brain.Screen.yPosition() <= 240 && Brain.Screen.yPosition() >= 0) {
task::sleep(500); //^if press was at D:[240<x<479] and R:[0<y<240] pixels, then...
//(Only occurs if screen has been pressed during this void)
Red_PH++;
Red_Select--;
Brain.Screen.clearScreen();
Brain.Screen.setCursor(1, 1);
Brain.Screen.print("Running 5-Point");
}
else {
task::sleep(0);
}
}
else {
task::sleep(0);
}
}
void Blue_Position() {
// Brain.Screen.clearScreen();
Brain.Screen.drawRectangle(0, 0, 240, 240, color::white);
Brain.Screen.printAt(119, 119, "4-Point");
Brain.Screen.drawRectangle(240, 0, 240, 240, color::black);
Brain.Screen.printAt(359, 119, "5-Point");
if (Brain.Screen.pressing()) {
if (Brain.Screen.xPosition() >= 0 && Brain.Screen.xPosition() <= 239 && Brain.Screen.yPosition() <= 240 && Brain.Screen.yPosition() >= 0) {
task::sleep(500);
Blue_4P++;
Blue_Select--;
Brain.Screen.clearScreen();
Brain.Screen.setCursor(1, 1);
Brain.Screen.print("Running 4-Point");
}
else if (Brain.Screen.xPosition() >= 240 && Brain.Screen.xPosition() <= 479 && Brain.Screen.yPosition() <= 240 && Brain.Screen.yPosition() >= 0) {
task::sleep(500);
Blue_PH++;
Blue_Select--;
Brain.Screen.clearScreen();
Brain.Screen.setCursor(1, 1);
Brain.Screen.print("Running 5-Point");
}
else {
task::sleep(0);
}
}
else {
task::sleep(0);
}
}
void Run_Red_4P() {
expand_2();
Driving_Intake(30, 35);
Backward(30, 26);
TR(30, 173);
Forward(20, 16);
T_Stack();
}
void Run_Red_5P() {
Forward(20, 10);
Backward(20, 10);
expand_2();
Driving_Intake(30, 37);
Backward(30, 28);
TR(30, 173);
Forward(20, 14);
T_Stack_NO();
}
void Run_Blue_4P() {
expand_2();
Driving_Intake(30, 37);
Backward(30, 28);
TL(30, 173);
Forward(20, 14);
T_Stack();
}
void Run_Blue_5P() {
Forward(20, 10);
Backward(20, 10);
expand_2();
Driving_Intake(30, 35);
Backward(30, 26);
TL(30, 173);
Forward(20, 14);
T_Stack_NO();
}
void pre_auton(void) {
//{
while (All == 1) {
Alliance();
}
while (Red_Select == 1) {
Red_Position();
}
while (Blue_Select == 1) {
Blue_Position();
}
//}
if (Red_PH == 1) {
Run_Red_5P();
}
if (Red_4P == 1) {
Run_Red_4P();
}
if (Blue_4P == 1) {
Run_Blue_4P();
}
if (Blue_PH == 1) {
Run_Blue_5P();
}
}
void autonomous( void ) {
pre_auton();
}
void usercontrol( void ) {
while (true) {
{
//Drive:
RD.spin(directionType::fwd, controller1.Axis2.value(), velocityUnits::pct);
LD.spin(directionType::fwd, controller1.Axis3.value(), velocityUnits::pct);
}
//Intake:
if(controller1.ButtonL1.pressing()){ //Intake
Intake.spin(directionType::fwd, 100, velocityUnits::pct);
}
else if(controller1.ButtonL2.pressing()){ //Outtake
Intake.spin(directionType::rev, 60, velocityUnits::pct);
}
else{
Intake.stop(brakeType::hold);
}
//Tilter:
if(controller1.ButtonR1.pressing()){ //Tilt back
T.spin(directionType::fwd, 100, velocityUnits::pct);
}
else if(controller1.ButtonR2.pressing()){ //Tilt Forward
T.spin(directionType::rev, 20, velocityUnits::pct);
}
else{
T.stop(brakeType::brake);
}
}
}
int main() {
Competition.autonomous( autonomous );
Competition.drivercontrol( usercontrol );
pre_auton();
while(1) {
vex::task::sleep(100);
}
}
Thank you @holbrook!