#pragma region VEXcode Generated Robot Configuration
// Make sure all required headers are included.
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <string.h>
#include "vex.h"
using namespace vex;
// Brain should be defined by default
brain Brain;
// START V5 MACROS
#define waitUntil(condition) \
do { \
wait(5, msec); \
} while (!(condition))
#define repeat(iterations) \
for (int iterator = 0; iterator < iterations; iterator++)
// END V5 MACROS
// Robot configuration code.
controller Controller1 = controller(primary);
motor leftMotorA = motor(PORT9, ratio6_1, false);
motor leftMotorB = motor(PORT2, ratio6_1, false);
motor_group LeftDriveSmart = motor_group(leftMotorA, leftMotorB);
motor rightMotorA = motor(PORT3, ratio6_1, true);
motor rightMotorB = motor(PORT4, ratio6_1, true);
motor_group RightDriveSmart = motor_group(rightMotorA, rightMotorB);
drivetrain Drivetrain = drivetrain(LeftDriveSmart, RightDriveSmart, 319.19, 295, 40, mm, 0.2);
motor intake = motor(PORT7, ratio18_1, false);
motor catapultMotorA = motor(PORT5, ratio36_1, true);
motor catapultMotorB = motor(PORT6, ratio36_1, false);
motor_group catapult = motor_group(catapultMotorA, catapultMotorB);
digital_out intakepneum = digital_out(Brain.ThreeWirePort.D);
digital_out wing1 = digital_out(Brain.ThreeWirePort.A);
digital_out wing2 = digital_out(Brain.ThreeWirePort.B);
// Helper to make playing sounds from the V5 in VEXcode easier and
// keeps the code cleaner by making it clear what is happening.
void playVexcodeSound(const char *soundName) {
printf("VEXPlaySound:%s\n", soundName);
wait(5, msec);
}
// define variable for remote controller enable/disable
bool RemoteControlCodeEnabled = true;
// define variables used for controlling motors based on controller inputs
bool Controller1LeftShoulderControlMotorsStopped = true;
bool Controller1RightShoulderControlMotorsStopped = true;
bool DrivetrainLNeedsToBeStopped_Controller1 = true;
bool DrivetrainRNeedsToBeStopped_Controller1 = true;
// define a task that will handle monitoring inputs from Controller1
int rc_auto_loop_function_Controller1() {
// process the controller input every 20 milliseconds
// update the motors based on the input values
while(true) {
if(RemoteControlCodeEnabled) {
// calculate the drivetrain motor velocities from the controller joystick axies
// left = Axis3 + Axis1
// right = Axis3 - Axis1
int drivetrainLeftSideSpeed = Controller1.Axis3.position() + Controller1.Axis1.position();
int drivetrainRightSideSpeed = Controller1.Axis3.position() - Controller1.Axis1.position();
// check if the value is inside of the deadband range
if (drivetrainLeftSideSpeed < 5 && drivetrainLeftSideSpeed > -5) {
// check if the left motor has already been stopped
if (DrivetrainLNeedsToBeStopped_Controller1) {
// stop the left drive motor
LeftDriveSmart.stop();
// tell the code that the left motor has been stopped
DrivetrainLNeedsToBeStopped_Controller1 = false;
}
} else {
// reset the toggle so that the deadband code knows to stop the left motor nexttime the input is in the deadband range
DrivetrainLNeedsToBeStopped_Controller1 = true;
}
// check if the value is inside of the deadband range
if (drivetrainRightSideSpeed < 5 && drivetrainRightSideSpeed > -5) {
// check if the right motor has already been stopped
if (DrivetrainRNeedsToBeStopped_Controller1) {
// stop the right drive motor
RightDriveSmart.stop();
// tell the code that the right motor has been stopped
DrivetrainRNeedsToBeStopped_Controller1 = false;
}
} else {
// reset the toggle so that the deadband code knows to stop the right motor next time the input is in the deadband range
DrivetrainRNeedsToBeStopped_Controller1 = true;
}
// only tell the left drive motor to spin if the values are not in the deadband range
if (DrivetrainLNeedsToBeStopped_Controller1) {
LeftDriveSmart.setVelocity(drivetrainLeftSideSpeed, percent);
LeftDriveSmart.spin(forward);
}
// only tell the right drive motor to spin if the values are not in the deadband range
if (DrivetrainRNeedsToBeStopped_Controller1) {
RightDriveSmart.setVelocity(drivetrainRightSideSpeed, percent);
RightDriveSmart.spin(forward);
}
// check the ButtonL1/ButtonL2 status to control intake
if (Controller1.ButtonL1.pressing()) {
intake.spin(forward);
Controller1LeftShoulderControlMotorsStopped = false;
} else if (Controller1.ButtonL2.pressing()) {
intake.spin(reverse);
Controller1LeftShoulderControlMotorsStopped = false;
} else if (!Controller1LeftShoulderControlMotorsStopped) {
intake.stop();
// set the toggle so that we don't constantly tell the motor to stop when the buttons are released
Controller1LeftShoulderControlMotorsStopped = true;
}
// check the ButtonR1/ButtonR2 status to control catapult
if (Controller1.ButtonR1.pressing()) {
catapult.spin(forward);
Controller1RightShoulderControlMotorsStopped = false;
} else if (Controller1.ButtonR2.pressing()) {
catapult.spin(reverse);
Controller1RightShoulderControlMotorsStopped = false;
} else if (!Controller1RightShoulderControlMotorsStopped) {
catapult.stop();
// set the toggle so that we don't constantly tell the motor to stop when the buttons are released
Controller1RightShoulderControlMotorsStopped = true;
}
}
// wait before repeating the process
wait(20, msec);
}
return 0;
}
task rc_auto_loop_task_Controller1(rc_auto_loop_function_Controller1);
#pragma endregion VEXcode Generated Robot Configuration
// Include the V5 Library
#include "vex.h"
// Allows for easier use of the VEX Library
using namespace vex;
competition Competition;
float myVariable;
// "when autonomous" hat block
int onauton_autonomous_0() {
// settings
Drivetrain.setDriveVelocity(10, percent);
Drivetrain.setTurnVelocity(10, percent);
intake.setVelocity(100, percent);
catapult.setVelocity(100, percent);
Drivetrain.setStopping(hold);
catapult.setStopping(hold);
Drivetrain.driveFor(forward, 15, inches);
Drivetrain.turnFor(right, 20, degrees);
Drivetrain.driveFor(forward, 7, inches);
Drivetrain.turnFor(left, 10, degrees);
Drivetrain.driveFor(forward, 6, inches);
Drivetrain.turnFor(left, 50, degrees);
wing1.set(true);
wing2.set(true);
Drivetrain.driveFor(forward, 13, inches);
while(true){
Brain.Screen.setPenColor("#ff0000");
Brain.Screen.setFillColor("#ff0000");
Brain.Screen.drawRectangle(0, 0, 479, 239);
Brain.Screen.setPenColor(black);
Brain.Screen.print("Æ");
wait(0.05, seconds);
Brain.Screen.setPenColor("#ff4700");
Brain.Screen.setFillColor("#ff4700");
Brain.Screen.drawRectangle(0, 0, 479, 239);
Brain.Screen.setPenColor(black);
Brain.Screen.print("Æ");
wait(0.05, seconds);
Brain.Screen.setPenColor("#ff8d00");
Brain.Screen.setFillColor("#ff8d00");
Brain.Screen.drawRectangle(0, 0, 479, 239);
Brain.Screen.setPenColor(black);
Brain.Screen.print("Æ");
wait(0.05, seconds);
Brain.Screen.setPenColor("#ffd400");
Brain.Screen.setFillColor("#ffd400");
Brain.Screen.drawRectangle(0, 0, 479, 239);
Brain.Screen.setPenColor(black);
Brain.Screen.print("Æ");
wait(0.05, seconds);
Brain.Screen.setPenColor("#c6ff00");
Brain.Screen.setFillColor("#c6ff00");
Brain.Screen.drawRectangle(0, 0, 479, 239);
Brain.Screen.setPenColor(black);
Brain.Screen.print("Æ");
wait(0.05, seconds);
Brain.Screen.setPenColor("#39ff00");
Brain.Screen.setFillColor("#39ff00");
Brain.Screen.drawRectangle(0, 0, 479, 239);
Brain.Screen.setPenColor(black);
Brain.Screen.print("Æ");
wait(0.05, seconds);
Brain.Screen.setPenColor("#00ff55");
Brain.Screen.setFillColor("#00ff55");
Brain.Screen.drawRectangle(0, 0, 479, 239);
Brain.Screen.setPenColor(black);
Brain.Screen.print("Æ");
wait(0.05, seconds);
Brain.Screen.setPenColor("#00ffe3");
Brain.Screen.setFillColor("#00ffe3");
Brain.Screen.drawRectangle(0, 0, 479, 239);
Brain.Screen.setPenColor(black);
Brain.Screen.print("Æ");
wait(0.05, seconds);
Brain.Screen.setPenColor("#6f8eff");
Brain.Screen.setFillColor("#6f8eff");
Brain.Screen.drawRectangle(0, 0, 479, 239);
Brain.Screen.setPenColor(black);
Brain.Screen.print("Æ");
wait(0.05, seconds);
Brain.Screen.setPenColor("#fa00ff");
Brain.Screen.setFillColor("#fa00ff");
Brain.Screen.drawRectangle(0, 0, 479, 239);
Brain.Screen.setPenColor(black);
Brain.Screen.print("Æ");
wait(0.05, seconds);
Brain.Screen.setPenColor("#6f8eff");
Brain.Screen.setFillColor("#6f8eff");
Brain.Screen.drawRectangle(0, 0, 479, 239);
Brain.Screen.setPenColor(black);
Brain.Screen.print("Æ");
wait(0.05, seconds);
Brain.Screen.setPenColor("#00ffe3");
Brain.Screen.setFillColor("#00ffe3");
Brain.Screen.drawRectangle(0, 0, 479, 239);
Brain.Screen.setPenColor(black);
Brain.Screen.print("Æ");
wait(0.05, seconds);
Brain.Screen.setPenColor("#00ff55");
Brain.Screen.setFillColor("#00ff55");
Brain.Screen.drawRectangle(0, 0, 479, 239);
Brain.Screen.setPenColor(black);
Brain.Screen.print("Æ");
wait(0.05, seconds);
Brain.Screen.setPenColor("#39ff00");
Brain.Screen.setFillColor("#39ff00");
Brain.Screen.drawRectangle(0, 0, 479, 239);
Brain.Screen.setPenColor(black);
Brain.Screen.print("Æ");
wait(0.05, seconds);
Brain.Screen.setPenColor("#c6ff00");
Brain.Screen.setFillColor("#c6ff00");
Brain.Screen.drawRectangle(0, 0, 479, 239);
Brain.Screen.setPenColor(black);
Brain.Screen.print("Æ");
wait(0.05, seconds);
Brain.Screen.setPenColor("#ffd400");
Brain.Screen.setFillColor("#ffd400");
Brain.Screen.drawRectangle(0, 0, 479, 239);
Brain.Screen.setPenColor(black);
Brain.Screen.print("Æ");
wait(0.05, seconds);
Brain.Screen.setPenColor("#ff8d00");
Brain.Screen.setFillColor("#ff8d00");
Brain.Screen.drawRectangle(0, 0, 479, 239);
Brain.Screen.setPenColor(black);
Brain.Screen.print("Æ");
wait(0.05, seconds);
Brain.Screen.setPenColor("#ff4700");
Brain.Screen.setFillColor("#ff4700");
Brain.Screen.drawRectangle(0, 0, 479, 239);
Brain.Screen.setPenColor(black);
Brain.Screen.print("Æ");
wait(0.05, seconds);
}
return 0;
}
// "when driver control" hat block
int ondriver_drivercontrol_0() {
Drivetrain.setDriveVelocity(100, percent);
Drivetrain.setTurnVelocity(100, percent);
intake.setVelocity(100, percent);
catapult.setVelocity(100, percent);
catapult.setStopping(hold);
Drivetrain.setStopping(coast);
while(true){
if (Controller1.ButtonA.pressing()){
wing1.set(true);
wing2.set(true);
}
else if (Controller1.ButtonY.pressing()){
wing1.set(false);
wing2.set(false);
}
if (Controller1.ButtonUp.pressing()){
intakepneum.set(true);
}
else if (Controller1.ButtonDown.pressing()){
intakepneum.set(false);
}
if (Controller1.ButtonLeft.pressing()){
Drivetrain.setStopping(hold);
}
else if (Controller1.ButtonRight.pressing()){
Drivetrain.setStopping(coast);
}
Brain.Screen.setPenColor("#ff0000");
Brain.Screen.setFillColor("#ff0000");
Brain.Screen.drawPixel(Controller1.Axis1.position() + 240, Controller1.Axis2.position() + 136);
wait(0.05, seconds);
Brain.Screen.setPenColor("#ff4700");
Brain.Screen.setFillColor("#ff4700");
Brain.Screen.drawPixel(Controller1.Axis1.position() + 240, Controller1.Axis2.position() + 136);
wait(0.05, seconds);
Brain.Screen.setPenColor("#ff8d00");
Brain.Screen.setFillColor("#ff8d00");
Brain.Screen.drawPixel(Controller1.Axis1.position() + 240, Controller1.Axis2.position() + 136);
wait(0.05, seconds);
Brain.Screen.setPenColor("#ffd400");
Brain.Screen.setFillColor("#ffd400");
Brain.Screen.drawPixel(Controller1.Axis1.position() + 240, Controller1.Axis2.position() + 136);
wait(0.05, seconds);
Brain.Screen.setPenColor("#c6ff00");
Brain.Screen.setFillColor("#c6ff00");
Brain.Screen.drawPixel(Controller1.Axis1.position() + 240, Controller1.Axis2.position() + 136);
wait(0.05, seconds);
Brain.Screen.setPenColor("#39ff00");
Brain.Screen.setFillColor("#39ff00");
Brain.Screen.drawPixel(Controller1.Axis1.position() + 240, Controller1.Axis2.position() + 136);
wait(0.05, seconds);
Brain.Screen.setPenColor("#00ff55");
Brain.Screen.setFillColor("#00ff55");
Brain.Screen.drawPixel(Controller1.Axis1.position() + 240, Controller1.Axis2.position() + 136);
wait(0.05, seconds);
Brain.Screen.setPenColor("#00ffe3");
Brain.Screen.setFillColor("#00ffe3");
Brain.Screen.drawPixel(Controller1.Axis1.position() + 240, Controller1.Axis2.position() + 136);
wait(0.05, seconds);
Brain.Screen.setPenColor("#6f8eff");
Brain.Screen.setFillColor("#6f8eff");
Brain.Screen.drawPixel(Controller1.Axis1.position() + 240, Controller1.Axis2.position() + 136);
wait(0.05, seconds);
Brain.Screen.setPenColor("#fa00ff");
Brain.Screen.setFillColor("#fa00ff");
Brain.Screen.drawPixel(Controller1.Axis1.position() + 240, Controller1.Axis2.position() + 136);
wait(0.05, seconds);
Brain.Screen.setPenColor("#6f8eff");
Brain.Screen.setFillColor("#6f8eff");
Brain.Screen.drawPixel(Controller1.Axis1.position() + 240, Controller1.Axis2.position() + 136);
wait(0.05, seconds);
Brain.Screen.setPenColor("#00ffe3");
Brain.Screen.setFillColor("#00ffe3");
Brain.Screen.drawPixel(Controller1.Axis1.position() + 240, Controller1.Axis2.position() + 136);
wait(0.05, seconds);
Brain.Screen.setPenColor("#00ff55");
Brain.Screen.setFillColor("#00ff55");
Brain.Screen.drawPixel(Controller1.Axis1.position() + 240, Controller1.Axis2.position() + 136);
wait(0.05, seconds);
Brain.Screen.setPenColor("#39ff00");
Brain.Screen.setFillColor("#39ff00");
Brain.Screen.drawPixel(Controller1.Axis1.position() + 240, Controller1.Axis2.position() + 136);
wait(0.05, seconds);
Brain.Screen.setPenColor("#c6ff00");
Brain.Screen.setFillColor("#c6ff00");
Brain.Screen.drawPixel(Controller1.Axis1.position() + 240, Controller1.Axis2.position() + 136);
wait(0.05, seconds);
Brain.Screen.setPenColor("#ffd400");
Brain.Screen.setFillColor("#ffd400");
Brain.Screen.drawPixel(Controller1.Axis1.position() + 240, Controller1.Axis2.position() + 136);
wait(0.05, seconds);
Brain.Screen.setPenColor("#ff8d00");
Brain.Screen.setFillColor("#ff8d00");
Brain.Screen.drawPixel(Controller1.Axis1.position() + 240, Controller1.Axis2.position() + 136);
wait(0.05, seconds);
Brain.Screen.setPenColor("#ff4700");
Brain.Screen.setFillColor("#ff4700");
Brain.Screen.drawPixel(Controller1.Axis1.position() + 240, Controller1.Axis2.position() + 136);
wait(0.05, seconds);
}
return 0;
}
void VEXcode_driver_task() {
// Start the driver control tasks....
vex::task drive0(ondriver_drivercontrol_0);
while(Competition.isDriverControl() && Competition.isEnabled()) {this_thread::sleep_for(10);}
drive0.stop();
return;
}
void VEXcode_auton_task() {
// Start the auton control tasks....
vex::task auto0(onauton_autonomous_0);
while(Competition.isAutonomous() && Competition.isEnabled()) {this_thread::sleep_for(10);}
auto0.stop();
return;
}
int main() {
vex::competition::bStopTasksBetweenModes = false;
Competition.autonomous(VEXcode_auton_task);
Competition.drivercontrol(VEXcode_driver_task);
// post event registration
// set default print color to black
printf("\033[30m");
// wait for rotation sensor to fully initialize
wait(30, msec);
}