diff --git a/EdgarProCompetition.v5code b/EdgarProCompetition.v5code index 4882cc8..0b0e8b4 100644 --- a/EdgarProCompetition.v5code +++ b/EdgarProCompetition.v5code @@ -1 +1 @@ -{"title":"EdgarProCompetition","description":"E.D.G.A.R. (Ergonomicaly Decent Goal-Grabbing Automated Robot)","icon":"USER921x.bmp","version":"21.10.0712","sdk":"20210708_10_00_00","language":"cpp","competition":false,"files":[{"name":"include/robot-config.h","type":"File","specialType":"device_config"},{"name":"include/vex.h","type":"File","specialType":""},{"name":"makefile","type":"File","specialType":""},{"name":"src/main.cpp","type":"File","specialType":""},{"name":"src/robot-config.cpp","type":"File","specialType":"device_config"},{"name":"vex/mkenv.mk","type":"File","specialType":""},{"name":"vex/mkrules.mk","type":"File","specialType":""},{"name":"CODE_OF_CONDUCT.md","type":"File","specialType":""},{"name":"index.md","type":"File","specialType":""},{"name":"include","type":"Directory"},{"name":"src","type":"Directory"},{"name":"vex","type":"Directory"}],"device":{"slot":1,"uid":"276-4810","options":{}},"isExpertMode":true,"isExpertModeRC":true,"isVexFileImport":false,"robotconfig":[],"neverUpdate":null} \ No newline at end of file +{"title":"EdgarProCompetition","description":"E.D.G.A.R. (Ergonomicaly Decent Goal-Grabbing Automated Robot)","icon":"USER921x.bmp","version":"21.10.0712","sdk":"20210708_10_00_00","language":"cpp","competition":false,"files":[{"name":"include/robot-config.h","type":"File","specialType":"device_config"},{"name":"include/vex.h","type":"File","specialType":""},{"name":"makefile","type":"File","specialType":""},{"name":"src/main.cpp","type":"File","specialType":""},{"name":"src/robot-config.cpp","type":"File","specialType":"device_config"},{"name":"vex/mkenv.mk","type":"File","specialType":""},{"name":"vex/mkrules.mk","type":"File","specialType":""},{"name":"CODE_OF_CONDUCT.md","type":"File","specialType":""},{"name":"index.md","type":"File","specialType":""},{"name":"CONTRIBUTING.md","type":"File","specialType":""},{"name":"include","type":"Directory"},{"name":"src","type":"Directory"},{"name":"vex","type":"Directory"}],"device":{"slot":1,"uid":"276-4810","options":{}},"isExpertMode":true,"isExpertModeRC":false,"isVexFileImport":false,"robotconfig":[{"port":[],"name":"Controller1","customName":false,"deviceType":"Controller","setting":{"left":"","leftDir":"false","right":"","rightDir":"false","upDown":"","upDownDir":"false","xB":"","xBDir":"false","drive":"none","id":"primary"},"triportSourcePort":22},{"port":[10],"name":"inertialSensor","customName":true,"deviceType":"Inertial","setting":{"id":"partner"},"triportSourcePort":22},{"port":[8],"name":"frontHook","customName":true,"deviceType":"DigitalOut","setting":{"id":"partner"},"triportSourcePort":22},{"port":[20],"name":"frontMogo","customName":true,"deviceType":"Motor","setting":{"reversed":"true","fwd":"forward","rev":"reverse","gear":"ratio36_1"},"triportSourcePort":22},{"port":[11],"name":"ringLift","customName":true,"deviceType":"Motor","setting":{"reversed":"false","fwd":"forward","rev":"reverse","gear":"ratio6_1"},"triportSourcePort":22},{"port":[1],"name":"LeftDriveSmartA","customName":true,"deviceType":"Motor","setting":{"reversed":"true","fwd":"forward","rev":"reverse","gear":"ratio6_1"},"triportSourcePort":22},{"port":[3],"name":"LeftDriveSmartB","customName":true,"deviceType":"Motor","setting":{"reversed":"false","fwd":"forward","rev":"reverse","gear":"ratio6_1"},"triportSourcePort":22},{"port":[9],"name":"LeftDriveSmartC","customName":true,"deviceType":"Motor","setting":{"reversed":"false","fwd":"forward","rev":"reverse","gear":"ratio6_1"},"triportSourcePort":22},{"port":[2],"name":"RightDriveSmartA","customName":true,"deviceType":"Motor","setting":{"reversed":"false","fwd":"forward","rev":"reverse","gear":"ratio6_1"},"triportSourcePort":22},{"port":[4],"name":"RightDriveSmartB","customName":true,"deviceType":"Motor","setting":{"reversed":"true","fwd":"forward","rev":"reverse","gear":"ratio6_1"},"triportSourcePort":22},{"port":[6],"name":"RightDriveSmartC","customName":true,"deviceType":"Motor","setting":{"reversed":"true","fwd":"forward","rev":"reverse","gear":"ratio6_1"},"triportSourcePort":22},{"port":[7],"name":"rearHook","customName":true,"deviceType":"DigitalOut","setting":{"id":"partner"},"triportSourcePort":22},{"port":[1],"name":"autonHook","customName":true,"deviceType":"DigitalOut","setting":{"id":"partner"},"triportSourcePort":22}],"neverUpdate":null} \ No newline at end of file diff --git a/compile_commands.json b/compile_commands.json index b933208..8315247 100644 --- a/compile_commands.json +++ b/compile_commands.json @@ -1 +1 @@ -[{"directory":"/Users/anthonyhalliday/Documents/vexcode-projects/Vex64040Edgar","file":"include/robot-config.h","arguments":["clang","-xc++","include/robot-config.h","-include","v5_vcs.h","--target=thumbv7-none-eabi","-U","__INT32_TYPE__","-U","__UINT32_TYPE__","-D","__INT32_TYPE__=long","-D","__UINT32_TYPE__=unsigned long","-std=gnu++11","-I","/Applications/VEXcode Pro V5.app/Contents/Resources/sdk/vexv5/include","-I","/Applications/VEXcode Pro V5.app/Contents/Resources/sdk/vexv5/clang/8.0.0/include","-I","/Applications/VEXcode Pro V5.app/Contents/Resources/sdk/vexv5/gcc/include","-I","/Applications/VEXcode Pro V5.app/Contents/Resources/sdk/vexv5/gcc/include/c++/4.9.3","-I","/Applications/VEXcode Pro V5.app/Contents/Resources/sdk/vexv5/gcc/include/c++/4.9.3/arm-none-eabi/armv7-ar/thumb"]},{"directory":"/Users/anthonyhalliday/Documents/vexcode-projects/Vex64040Edgar","file":"include/vex.h","arguments":["clang","-xc++","include/vex.h","-include","v5_vcs.h","--target=thumbv7-none-eabi","-U","__INT32_TYPE__","-U","__UINT32_TYPE__","-D","__INT32_TYPE__=long","-D","__UINT32_TYPE__=unsigned long","-std=gnu++11","-I","/Applications/VEXcode Pro V5.app/Contents/Resources/sdk/vexv5/include","-I","/Applications/VEXcode Pro V5.app/Contents/Resources/sdk/vexv5/clang/8.0.0/include","-I","/Applications/VEXcode Pro V5.app/Contents/Resources/sdk/vexv5/gcc/include","-I","/Applications/VEXcode Pro V5.app/Contents/Resources/sdk/vexv5/gcc/include/c++/4.9.3","-I","/Applications/VEXcode Pro V5.app/Contents/Resources/sdk/vexv5/gcc/include/c++/4.9.3/arm-none-eabi/armv7-ar/thumb"]},{"directory":"/Users/anthonyhalliday/Documents/vexcode-projects/Vex64040Edgar","file":"src/main.cpp","output":"build/src/main.o","arguments":["clang","-xc++","src/main.cpp","--target=thumbv7-none-eabi","-fshort-enums","-Wno-unknown-attributes","-U","__INT32_TYPE__","-U","__UINT32_TYPE__","-D","__INT32_TYPE__=long","-D","__UINT32_TYPE__=unsigned long","-march=armv7-a","-mfpu=neon","-mfloat-abi=softfp","-Os","-Wall","-Werror=return-type","-fno-rtti","-fno-threadsafe-statics","-fno-exceptions","-std=gnu++11","-ffunction-sections","-fdata-sections","-D","VexV5","-I","/Users/anthonyhalliday/Documents/vexcode-projects/Vex64040Edgar","-I","/Users/anthonyhalliday/Documents/vexcode-projects/Vex64040Edgar/include","-I","/Users/anthonyhalliday/Documents/vexcode-projects/Vex64040Edgar/src","-I","/Applications/VEXcode Pro V5.app/Contents/Resources/sdk/vexv5/include","-I","/Applications/VEXcode Pro V5.app/Contents/Resources/sdk/vexv5/clang/8.0.0/include","-I","/Applications/VEXcode Pro V5.app/Contents/Resources/sdk/vexv5/gcc/include","-I","/Applications/VEXcode Pro V5.app/Contents/Resources/sdk/vexv5/gcc/include/c++/4.9.3","-I","/Applications/VEXcode Pro V5.app/Contents/Resources/sdk/vexv5/gcc/include/c++/4.9.3/arm-none-eabi/armv7-ar/thumb","-c","-o","build/src/main.o","-mlinker-version=409.12","--target=armv7-none--eabi"]},{"directory":"/Users/anthonyhalliday/Documents/vexcode-projects/Vex64040Edgar","file":"src/robot-config.cpp","output":"build/src/robot-config.o","arguments":["clang","-xc++","src/robot-config.cpp","--target=thumbv7-none-eabi","-fshort-enums","-Wno-unknown-attributes","-U","__INT32_TYPE__","-U","__UINT32_TYPE__","-D","__INT32_TYPE__=long","-D","__UINT32_TYPE__=unsigned long","-march=armv7-a","-mfpu=neon","-mfloat-abi=softfp","-Os","-Wall","-Werror=return-type","-fno-rtti","-fno-threadsafe-statics","-fno-exceptions","-std=gnu++11","-ffunction-sections","-fdata-sections","-D","VexV5","-I","/Users/anthonyhalliday/Documents/vexcode-projects/Vex64040Edgar","-I","/Users/anthonyhalliday/Documents/vexcode-projects/Vex64040Edgar/include","-I","/Users/anthonyhalliday/Documents/vexcode-projects/Vex64040Edgar/src","-I","/Applications/VEXcode Pro V5.app/Contents/Resources/sdk/vexv5/include","-I","/Applications/VEXcode Pro V5.app/Contents/Resources/sdk/vexv5/clang/8.0.0/include","-I","/Applications/VEXcode Pro V5.app/Contents/Resources/sdk/vexv5/gcc/include","-I","/Applications/VEXcode Pro V5.app/Contents/Resources/sdk/vexv5/gcc/include/c++/4.9.3","-I","/Applications/VEXcode Pro V5.app/Contents/Resources/sdk/vexv5/gcc/include/c++/4.9.3/arm-none-eabi/armv7-ar/thumb","-c","-o","build/src/robot-config.o","-mlinker-version=409.12","--target=armv7-none--eabi"]}] \ No newline at end of file +[{"directory":"C:/Users/stroebell/Documents/GitHub/Edgar-Pro","file":"include/robot-config.h","arguments":["clang","-xc++","include/robot-config.h","-include","v5_vcs.h","--target=thumbv7-none-eabi","-U","__INT32_TYPE__","-U","__UINT32_TYPE__","-D","__INT32_TYPE__=long","-D","__UINT32_TYPE__=unsigned long","-std=gnu++11","-I","C:/Program Files (x86)/VEX Robotics/VEXcode Pro V5/sdk/vexv5/include","-I","C:/Program Files (x86)/VEX Robotics/VEXcode Pro V5/sdk/vexv5/clang/8.0.0/include","-I","C:/Program Files (x86)/VEX Robotics/VEXcode Pro V5/sdk/vexv5/gcc/include","-I","C:/Program Files (x86)/VEX Robotics/VEXcode Pro V5/sdk/vexv5/gcc/include/c++/4.9.3","-I","C:/Program Files (x86)/VEX Robotics/VEXcode Pro V5/sdk/vexv5/gcc/include/c++/4.9.3/arm-none-eabi/armv7-ar/thumb"]},{"directory":"C:/Users/stroebell/Documents/GitHub/Edgar-Pro","file":"include/vex.h","arguments":["clang","-xc++","include/vex.h","-include","v5_vcs.h","--target=thumbv7-none-eabi","-U","__INT32_TYPE__","-U","__UINT32_TYPE__","-D","__INT32_TYPE__=long","-D","__UINT32_TYPE__=unsigned long","-std=gnu++11","-I","C:/Program Files (x86)/VEX Robotics/VEXcode Pro V5/sdk/vexv5/include","-I","C:/Program Files (x86)/VEX Robotics/VEXcode Pro V5/sdk/vexv5/clang/8.0.0/include","-I","C:/Program Files (x86)/VEX Robotics/VEXcode Pro V5/sdk/vexv5/gcc/include","-I","C:/Program Files (x86)/VEX Robotics/VEXcode Pro V5/sdk/vexv5/gcc/include/c++/4.9.3","-I","C:/Program Files (x86)/VEX Robotics/VEXcode Pro V5/sdk/vexv5/gcc/include/c++/4.9.3/arm-none-eabi/armv7-ar/thumb"]},{"directory":"C:/Users/stroebell/Documents/GitHub/Edgar-Pro","file":"src/main.cpp","output":"build/src/main.o","arguments":["clang","-xc++","src/main.cpp","--target=thumbv7-none-eabi","-fshort-enums","-Wno-unknown-attributes","-U","__INT32_TYPE__","-U","__UINT32_TYPE__","-D","__INT32_TYPE__=long","-D","__UINT32_TYPE__=unsigned long","-march=armv7-a","-mfpu=neon","-mfloat-abi=softfp","-Os","-Wall","-Werror=return-type","-fno-rtti","-fno-threadsafe-statics","-fno-exceptions","-std=gnu++11","-ffunction-sections","-fdata-sections","-D","VexV5","-I","C:/Users/stroebell/Documents/GitHub/Edgar-Pro","-I","C:/Users/stroebell/Documents/GitHub/Edgar-Pro/include","-I","C:/Users/stroebell/Documents/GitHub/Edgar-Pro/src","-I","C:/Program Files (x86)/VEX Robotics/VEXcode Pro V5/sdk/vexv5/include","-I","C:/Program Files (x86)/VEX Robotics/VEXcode Pro V5/sdk/vexv5/clang/8.0.0/include","-I","C:/Program Files (x86)/VEX Robotics/VEXcode Pro V5/sdk/vexv5/gcc/include","-I","C:/Program Files (x86)/VEX Robotics/VEXcode Pro V5/sdk/vexv5/gcc/include/c++/4.9.3","-I","C:/Program Files (x86)/VEX Robotics/VEXcode Pro V5/sdk/vexv5/gcc/include/c++/4.9.3/arm-none-eabi/armv7-ar/thumb","-c","-o","build/src/main.o","-mlinker-version=409.12","--target=armv7-none--eabi"]},{"directory":"C:/Users/stroebell/Documents/GitHub/Edgar-Pro","file":"src/robot-config.cpp","output":"build/src/robot-config.o","arguments":["clang","-xc++","src/robot-config.cpp","--target=thumbv7-none-eabi","-fshort-enums","-Wno-unknown-attributes","-U","__INT32_TYPE__","-U","__UINT32_TYPE__","-D","__INT32_TYPE__=long","-D","__UINT32_TYPE__=unsigned long","-march=armv7-a","-mfpu=neon","-mfloat-abi=softfp","-Os","-Wall","-Werror=return-type","-fno-rtti","-fno-threadsafe-statics","-fno-exceptions","-std=gnu++11","-ffunction-sections","-fdata-sections","-D","VexV5","-I","C:/Users/stroebell/Documents/GitHub/Edgar-Pro","-I","C:/Users/stroebell/Documents/GitHub/Edgar-Pro/include","-I","C:/Users/stroebell/Documents/GitHub/Edgar-Pro/src","-I","C:/Program Files (x86)/VEX Robotics/VEXcode Pro V5/sdk/vexv5/include","-I","C:/Program Files (x86)/VEX Robotics/VEXcode Pro V5/sdk/vexv5/clang/8.0.0/include","-I","C:/Program Files (x86)/VEX Robotics/VEXcode Pro V5/sdk/vexv5/gcc/include","-I","C:/Program Files (x86)/VEX Robotics/VEXcode Pro V5/sdk/vexv5/gcc/include/c++/4.9.3","-I","C:/Program Files (x86)/VEX Robotics/VEXcode Pro V5/sdk/vexv5/gcc/include/c++/4.9.3/arm-none-eabi/armv7-ar/thumb","-c","-o","build/src/robot-config.o","-mlinker-version=409.12","--target=armv7-none--eabi"]}] \ No newline at end of file diff --git a/include/robot-config.h b/include/robot-config.h index 5a0a859..7b0bec3 100644 --- a/include/robot-config.h +++ b/include/robot-config.h @@ -3,13 +3,17 @@ using namespace vex; extern brain Brain; // VEXcode devices -extern digital_out frontHook; +extern controller Controller1; extern inertial inertialSensor; +extern digital_out frontHook; extern motor frontMogo; extern motor ringLift; -extern motor_group LeftDriveSmart; -extern motor_group RightDriveSmart; -extern controller Controller1; +extern motor LeftDriveSmartA; +extern motor LeftDriveSmartB; +extern motor LeftDriveSmartC; +extern motor RightDriveSmartA; +extern motor RightDriveSmartB; +extern motor RightDriveSmartC; extern digital_out rearHook; extern digital_out autonHook; diff --git a/src/main.cpp b/src/main.cpp index a4bd633..2f0e2f5 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -9,42 +9,16 @@ // "It's always the programmer's fault" - Some genius on the vex fourms -// ---- START VEXCODE CONFIGURED DEVICES ---- -// Robot Configuration: -// [Name] [Type] [Port(s)] -// frontHook digital_out H -// rearHook digital_out G -// inertialSensor inertial 10 -// frontMogo motor 7 -// ringLift motor 8 -// LeftDriveSmart motor_group 1, 3, 5 -// RightDriveSmart motor_group 2, 4, 6 -// Controller1 controller -// ---- END VEXCODE CONFIGURED DEVICES ---- - -// ---- START SUPPORTED AUTON ROUTES ---- -// [Name] [Support Level] -// LANWP Incomplete -// SPEED Complete -// RANWP Complete -// RAR Complete -// SKILL In Progress -// ZACH1 In Progress -// ZACH2 Not Started -// ZACH3 Not Started -// ZACH4 Not Started -// ZACH5 Not Started -// ZACH6 Not Started -// ZACH7 Not Started -// ZACH8 Not Started -// ---- END SUPPORTED AUTON ROUTES ---- - #include "vex.h" #include "cmath" using namespace vex; competition Competition; +// Manualy create a motor group because the vex config wizzard can't have more than two motors for some reason +motor_group LeftDriveSmart = motor_group(LeftDriveSmartA, LeftDriveSmartB, LeftDriveSmartC); +motor_group RightDriveSmart = motor_group(RightDriveSmartA, RightDriveSmartB, RightDriveSmartC); + // define variables and macros here int calcVelocity; int prevTurn; @@ -53,17 +27,17 @@ float heading; #define MAXVELOCITY 40 #define MINVELOCITY 10 -// define functions here +// Essential driving functions void drive2obs(directionType dir){ - LeftDriveSmart.spin(dir,200,velocityUnits::pct); - RightDriveSmart.spin(dir,200,velocityUnits::pct); + LeftDriveSmart.spin(dir,12.0,voltageUnits::volt); + RightDriveSmart.spin(dir,12.0,voltageUnits::volt); if(dir==directionType::fwd){ waitUntil(LeftDriveSmart.velocity(percentUnits::pct)>5 && RightDriveSmart.velocity(percentUnits::pct)>5); waitUntil(LeftDriveSmart.velocity(percentUnits::pct)<5 && RightDriveSmart.velocity(percentUnits::pct)<5); } else{ - waitUntil(LeftDriveSmart.velocity(percentUnits::pct)<5 && RightDriveSmart.velocity(percentUnits::pct)<5); - waitUntil(LeftDriveSmart.velocity(percentUnits::pct)>5 && RightDriveSmart.velocity(percentUnits::pct)>5); + waitUntil(LeftDriveSmart.velocity(percentUnits::pct)<-5 && RightDriveSmart.velocity(percentUnits::pct)<-5); + waitUntil(LeftDriveSmart.velocity(percentUnits::pct)>-5 || RightDriveSmart.velocity(percentUnits::pct)>-5); } LeftDriveSmart.stop(); RightDriveSmart.stop(); @@ -152,179 +126,230 @@ void driveIN(float dist, directionType dir,float volt) { } // define auton routines here -void rightAutonRight(void) { - // right auton code goes here - // open claw, drive forward to neutral mogo, latch on and lift - frontHook.set(false); - driveIN(47,directionType::fwd,12.0); - frontMogo.spinFor(150,rotationUnits::deg,false); - frontHook.set(true); - driveIN(15,directionType::rev,12.0); - InertialLeft(40); - driveIN(20,directionType::rev,12.0); - rearHook.set(true); - ringLift.spinFor(3,timeUnits::sec,100,velocityUnits::pct); +void SKILL() { + rearHook.set(false); + // insert camden code here } -void skillsAuton(void) { - frontHook.set(false); - driveIN(10,directionType::fwd,55); - //rearMogo.spinTo(-700,rotationUnits::deg); // Depreciated [DELETE] - driveIN(11,directionType::rev,55); - rearHook.set(true); - ringLift.spinFor(2,timeUnits::sec,100,velocityUnits::pct); - driveIN(10,directionType::fwd,55); - InertialRight(95); - driveIN(50,directionType::fwd,55); - frontHook.set(true); - frontMogo.spinFor(300,rotationUnits::deg); - InertialRight(10); - driveIN(30,directionType::fwd,55); - frontMogo.spinFor(-300,rotationUnits::deg); - frontHook.set(false); - InertialLeft(90); +void SPEED() { rearHook.set(false); - driveIN(10,directionType::fwd,55); - InertialRight(180); - driveIN(13,directionType::rev,55); - rearHook.set(true); - InertialRight(11); - driveIN(45,directionType::fwd,55); + Brain.Screen.setFont(fontType::mono60); + Brain.Screen.printAt(75,100,"prankd, lol"); + driveIN(47,directionType::fwd,12.0); + LeftDriveSmart.spin(directionType::rev,200,velocityUnits::pct); + RightDriveSmart.spin(directionType::rev,200,velocityUnits::pct); frontHook.set(true); - driveIN(35,directionType::fwd,55); - InertialRight(45); - drive2obs(directionType::fwd); - + frontMogo.spinFor(90,rotationUnits::deg,false); + task::sleep(10000); + Brain.Screen.clearScreen(); } -void leftAutonNoWP(void){ - //rearMogo.spinTo(-650,rotationUnits::deg,200, velocityUnits::pct,false); //Depreciated [DELETE] - driveIN(47,directionType::fwd,12.0); +// Right Side +void RANWP(){ + rearHook.set(false); + // open claw, drive forward to neutral mogo, latch on and lift + driveIN(55,directionType::fwd,150); frontHook.set(true); - frontMogo.spinTo(110,rotationUnits::deg); - InertialLeft(95); - driveIN(27,directionType::rev,30); + frontMogo.setBrake(brakeType::hold); + frontMogo.startSpinTo(800,rotationUnits::deg); + InertialRight(35); + driveIN(25,directionType::rev,12.0); + InertialRight(65); + driveIN(20,directionType::rev,7.0); rearHook.set(true); - InertialLeft(45); - driveIN(30,directionType::fwd,70); - InertialRight(180); - driveIN(30,directionType::rev,70); + driveIN(50,directionType::fwd,12.0); } -void rightAutonNoWP(void){ - // open claw, drive forward to neutral mogo, latch on and lift +void RADWA(){ + rearHook.set(false); + // start on right, speed for right yellow with front, grab center yellow with back, place center yellow in corner, grab alliance with back, get field rings driveIN(55,directionType::fwd,150); frontHook.set(true); - frontMogo.spinFor(150,rotationUnits::deg,false); - InertialRight(45); - driveIN(15,directionType::rev,12.0); - InertialRight(50); - driveIN(20,directionType::rev,12.0); + frontMogo.setBrake(brakeType::hold); + frontMogo.startSpinTo(800,rotationUnits::deg); + InertialRight(35); + driveIN(25,directionType::rev,12.0); + InertialRight(65); + driveIN(18,directionType::rev,7.0); + + LeftDriveSmart.spin(directionType::rev,6.0,voltageUnits::volt); + RightDriveSmart.spin(directionType::rev,6.0,voltageUnits::volt); + LeftDriveSmart.resetPosition(); + RightDriveSmart.resetPosition(); + // 5.75 in/rot + + waitUntil(LeftDriveSmart.position(rotationUnits::rev)<2.875 && RightDriveSmart.position(rotationUnits::rev)<2.875); rearHook.set(true); - InertialRight(45); - driveIN(50,directionType::fwd,12.0); + waitUntil(LeftDriveSmart.position(rotationUnits::rev)<3.5 && RightDriveSmart.position(rotationUnits::rev)<3.5); + + driveIN(32,directionType::fwd,12.0); + InertialLeft(110); + driveIN(12,directionType::rev,7.0); + rearHook.set(false); + task::sleep(1000); + InertialLeft(85); + drive2obs(directionType::rev); + rearHook.set(true); + ringLift.spin(directionType::fwd,12.0,voltageUnits::volt); } -void speedyAuton(void) { - Controller1.Screen.print(" PRANKD LOL"); +void RARNR() { + rearHook.set(false); + // open claw, drive forward to neutral mogo, latch on and lift + frontHook.set(false); driveIN(47,directionType::fwd,12.0); - LeftDriveSmart.spin(directionType::rev,200,velocityUnits::pct); - RightDriveSmart.spin(directionType::rev,200,velocityUnits::pct); + frontMogo.spinFor(150,rotationUnits::deg,false); frontHook.set(true); - frontMogo.spinFor(90,rotationUnits::deg,false); + driveIN(15,directionType::rev,12.0); + InertialLeft(40); + driveIN(20,directionType::rev,12.0); + rearHook.set(true); + InertialRight(10); + ringLift.spinFor(3,timeUnits::sec,100,velocityUnits::pct); } -void zach1(){ +void RARWR(){ + rearHook.set(false); + // Start on right, grab right neutral with front, grab alliance goal with back, load with rings from field frontHook.set(false); driveIN(47,directionType::fwd,12.0); - frontMogo.spinFor(700,rotationUnits::deg,200,velocityUnits::pct,false); frontHook.set(true); - frontMogo.stop(brakeType::hold); + frontMogo.setBrake(brakeType::hold); + frontMogo.startSpinTo(800,rotationUnits::deg,200,velocityUnits::pct); driveIN(50,directionType::rev,12.0); InertialLeft(125); - driveIN(15,directionType::rev,7.0); + driveIN(17,directionType::rev,7.0); rearHook.set(true); task::sleep(500); - ringLift.spin(directionType::fwd,100,velocityUnits::pct); - driveIN(4.5,directionType::fwd,12.0); + frontMogo.stop(brakeType::hold); + ringLift.spin(directionType::fwd,50,velocityUnits::pct); + driveIN(4.75,directionType::fwd,12.0); InertialRight(125); - driveIN(55,directionType::fwd,6.75); - driveIN(70,directionType::rev,12.0); - ringLift.stop(); + ringLift.setVelocity(200, percentUnits::pct); + driveIN(55,directionType::fwd,5.5); + driveIN(55,directionType::rev,5.5); + InertialLeft(200); } -void zach2(){ - +void RACWR(){ + rearHook.set(false); + // start on right, grab yellow center with front, grab alliance goal with rear, load with rings from field + driveIN(60,directionType::fwd,12.0); + frontHook.set(true); + frontMogo.setBrake(brakeType::hold); + frontMogo.startSpinTo(800,rotationUnits::deg,200,velocityUnits::pct); + driveIN(43,directionType::rev,12.0); + task::sleep(500); + InertialLeft(42.5); + driveIN(15,directionType::rev,7.0); + rearHook.set(true); + driveIN(5,directionType::fwd,12.0); + InertialRight(95); + ringLift.spin(directionType::fwd,12.0,voltageUnits::volt); + driveIN(55,directionType::fwd,7.0); + driveIN(70,directionType::rev,12.0); } -void zach3(){ - -} +void RACRF(){ + rearHook.set(false); + // same as RACWR, but fake for right goal instead of center + InertialLeft(30); + driveIN(65,directionType::fwd,7.0); + frontHook.set(true); + frontMogo.setBrake(brakeType::hold); + frontMogo.startSpinTo(800,rotationUnits::deg,200,velocityUnits::pct); + driveIN(42,directionType::rev,12.0); + task::sleep(500); + InertialLeft(40); -void zach4(){ - -} + LeftDriveSmart.spin(directionType::rev,6.0,voltageUnits::volt); + RightDriveSmart.spin(directionType::rev,6.0,voltageUnits::volt); + task::sleep(100); + LeftDriveSmart.spin(directionType::rev,8.0,voltageUnits::volt); + RightDriveSmart.spin(directionType::rev,8.0,voltageUnits::volt); + task::sleep(100); + LeftDriveSmart.spin(directionType::rev,10.0,voltageUnits::volt); + RightDriveSmart.spin(directionType::rev,10.0,voltageUnits::volt); + task::sleep(100); + drive2obs(directionType::rev); -void zach5(){ - + task::sleep(500); + rearHook.set(true); + driveIN(5,directionType::fwd,12.0); + InertialRight(95); + ringLift.spin(fwd,12.0,voltageUnits::volt); + driveIN(50,directionType::fwd,7.0); + driveIN(60,directionType::rev,12.0); } -void zach6(){ - -} +// Left side +void LANWP(){ + rearHook.set(false); + driveIN(47,directionType::fwd,12.0); + frontHook.set(true); + frontMogo.startSpinTo(110,rotationUnits::deg); -void zach7(){ - -} + InertialLeft(35); + driveIN(20,directionType::rev,12.0); + InertialLeft(115); + driveIN(35,directionType::rev,7.0); -void zach8(){ - + rearHook.set(true); + InertialLeft(15); + driveIN(60,directionType::fwd,70); } -// now that autons are defined, we can define the auton selection code -std::string autonRoutes [13] = {"LANWP","SPEED","RANWP","RAR","SKILL","ZACH1","ZACH2","ZACH3","ZACH4","ZACH5","ZACH6","ZACH7","ZACH8"}; -bool waitForComplete = true; -int autonIndex = 0; - -void autonSelect(){ - printf("%i",autonIndex); - while(waitForComplete){ - if (Controller1.ButtonUp.pressing()){ - autonIndex++; - printf("%i",autonIndex); - waitUntil(!Controller1.ButtonUp.pressing()); - } - else if (Controller1.ButtonDown.pressing()){ - autonIndex--; - printf("%i",autonIndex); - waitUntil(!Controller1.ButtonDown.pressing()); - } - else if (Controller1.ButtonX.pressing()) { - waitForComplete = false; - } - - // prevent overflow - autonIndex = autonIndex < 0 ? autonIndex+13 : autonIndex; - autonIndex = autonIndex > 12 ? 0 : autonIndex; - - } +void LALWR(){ + rearHook.set(false); + // this routine is still very much a work in progress, so don't be supprised if it doesn't perform as expected + // start on left, speed for left yellow, back up to alliance on platform, grab, load with match load rings + driveIN(48,directionType::fwd,12.0); + frontHook.set(true); + frontMogo.startSpinTo(800,rotationUnits::deg); + driveIN(34,directionType::rev,12.0); + InertialRight(10); + driveIN(15,directionType::rev,6.0); + InertialLeft(120); + drive2obs(directionType::fwd); + driveIN(30,directionType::rev,6.0); + rearHook.set(true); + driveIN(20,directionType::fwd,7.0); + InertialLeft(90); + ringLift.spin(directionType::fwd, 12.0, voltageUnits::volt); + driveIN(24,directionType::rev,12.0); + driveIN(24,directionType::fwd,7.0); } -// define pre-auton routine here -void pre_auton(void) { - // Initializing Robot Configuration. DO NOT REMOVE! - vexcodeInit(); - // Reset important encoders, close the front claw, and open the back - frontHook.set(false); +void LACRF(){ rearHook.set(false); - frontMogo.resetPosition(); - // Calibrate Inertial and wait until complete - inertialSensor.startCalibration(); - waitUntil(!inertialSensor.isCalibrating()); + // start on left, fake for left yellow, go for center, bring it back, grab alliance with rear lift, load with match loads + InertialRight(25); + + ringLift.spin(directionType::fwd,12.0,voltageUnits::volt); + frontMogo.startSpinTo(200, rotationUnits::deg, 200, velocityUnits::pct); + driveIN(30,directionType::fwd,12.0); + driveIN(30,directionType::fwd,3.0); + frontMogo.startSpinTo(0, rotationUnits::deg, 200, velocityUnits::pct); + driveIN(15,directionType::fwd,6.0); + frontHook.set(true); + frontMogo.startSpinTo(800, rotationUnits::deg, 200, velocityUnits::pct); + + driveIN(10,directionType::rev,12.0); + driveIN(60,directionType::rev,5.0); + frontMogo.spin(directionType::fwd); + InertialLeft(160); + + drive2obs(directionType::fwd); + driveIN(40,directionType::rev,5.0); + rearHook.set(true); + driveIN(25,directionType::fwd,4.0); + InertialLeft(90); + driveIN(30,directionType::rev,5.0); + driveIN(60,directionType::fwd,5.0); } // define user control code here -void pneumaticSwitchFront(void) { +void pneumaticSwitchFront() { // on button press, check if driver control is active, then if front hook is pressurized, depresurize, otherwise, pressurize if(competition().isDriverControl()){ if(frontHook.value()) { @@ -336,13 +361,17 @@ void pneumaticSwitchFront(void) { } } -void usercontrol(void) { +void usercontrol() { // Bind button x to front hook Controller1.ButtonX.pressed(pneumaticSwitchFront); while(true) { // Spin drivetrain motors LeftDriveSmart.spin(vex::directionType::undefined, (2*abs(Controller1.Axis3.position()+Controller1.Axis4.position()) >= DEADBAND) ? 0-(Controller1.Axis3.position()+Controller1.Axis4.position()) : 0, velocityUnits::pct); RightDriveSmart.spin(vex::directionType::undefined, (2*abs(Controller1.Axis3.position()-Controller1.Axis4.position()) >= DEADBAND) ? 0-(Controller1.Axis3.position()-Controller1.Axis4.position()) : 0, velocityUnits::pct); + // Test code, don't uncomment unless you know what you're doing + //RightDriveSmart.spin(directionType::undefined,Controller1.Axis3.position(),velocityUnits::pct); + //LeftDriveSmart.spin(directionType::undefined,Controller1.Axis3.position(),velocityUnits::pct); + // Rear Lift up/down if(Controller1.ButtonL1.pressing()) { rearHook.set(true); @@ -375,17 +404,89 @@ void usercontrol(void) { // main() called on program start int main() { - // run the pre-auton routine - pre_auton(); - // Manualy select an auton - Competition.autonomous(zach1); - // Autonomous Selection - //autonSelect(); - // Set up callbacks for driver control period. + // Initializing Robot Configuration. DO NOT REMOVE! + vexcodeInit(); + + // Set color and font for screen + Brain.Screen.setFont(fontType::mono60); + Brain.Screen.setFillColor(red); + + // Reset important encoders, open the front claw, and close the back claw + frontHook.set(false); + rearHook.set(true); + frontMogo.resetPosition(); + + // Calibrate Inertial and wait until complete + inertialSensor.startCalibration(); + waitUntil(!inertialSensor.isCalibrating()); + + // Super Fancy Auton Selector (C) Anthony Halliday, 2022, Under the license "pls dont steal my code, thx" + select: + if (Controller1.ButtonUp.pressing()){ + Competition.autonomous(SPEED); + Controller1.Screen.print("SPEED"); + } + else if (Controller1.ButtonDown.pressing()){ + Competition.autonomous(LANWP); + Controller1.Screen.print("LANWP"); + } + else if (Controller1.ButtonLeft.pressing()){ + Competition.autonomous(LALWR); + Controller1.Screen.print("LALWR"); + } + else if (Controller1.ButtonRight.pressing()){ + Competition.autonomous(LACRF); + Controller1.Screen.print("LACRF"); + } + else if (Controller1.ButtonX.pressing()){ + Competition.autonomous(RANWP); + Controller1.Screen.print("RANWP"); + } + else if (Controller1.ButtonY.pressing()){ + Competition.autonomous(RACRF); + Controller1.Screen.print("RACRF"); + } + else if (Controller1.ButtonA.pressing()){ + Competition.autonomous(RACWR); + Controller1.Screen.print("RACWR"); + } + else if (Controller1.ButtonB.pressing()){ + Competition.autonomous(RADWA); + Controller1.Screen.print("RADWA"); + } + else if (Controller1.ButtonL1.pressing()){ + Competition.autonomous(SKILL); + Controller1.Screen.print("SKILL"); + } + else if (Controller1.ButtonR1.pressing()){ + Competition.autonomous(RARNR); + Controller1.Screen.print("RARNR"); + } + else if (Controller1.ButtonR2.pressing()){ + Competition.autonomous(RARWR); + Controller1.Screen.print("RARWR"); + } + else{ + goto select; + } + // jk lol, its just completley broken, welcome to vex! + task::sleep(2000); + Controller1.Screen.clearLine(4); + // Set up driver control period. Competition.drivercontrol(usercontrol); + // Prevent main from exiting with an infinite loop. while(true) { - task::sleep(100); + if (LeftDriveSmart.temperature(temperatureUnits::celsius)>50 || RightDriveSmart.temperature(temperatureUnits::celsius)>50){ + //Brain.Screen.drawRectangle(0, 0, 480, 240); + Brain.Screen.printAt(120,100,"OVERHEAT"); + + while (LeftDriveSmart.temperature(temperatureUnits::celsius)>50 || RightDriveSmart.temperature(temperatureUnits::celsius)>50){ + Controller1.rumble("---"); + } + } + task::sleep(1000); + } } diff --git a/src/robot-config.cpp b/src/robot-config.cpp index 25f68d4..5151862 100644 --- a/src/robot-config.cpp +++ b/src/robot-config.cpp @@ -8,25 +8,17 @@ using code = vision::code; brain Brain; // VEXcode device constructors - controller Controller1 = controller(primary); inertial inertialSensor = inertial(PORT10); - digital_out frontHook = digital_out(Brain.ThreeWirePort.H); -motor frontMogo = motor(PORT7, ratio36_1, true); - -motor ringLift = motor(PORT11, ratio18_1, false); //FIX GEARS LMAO - -motor LeftDriveSmartMotorA = motor(PORT1, ratio6_1, true); -motor LeftDriveSmartMotorB = motor(PORT3, ratio6_1, false); -motor LeftDriveSmartMotorC = motor(PORT9, ratio6_1, false); -motor_group LeftDriveSmart = motor_group(LeftDriveSmartMotorA, LeftDriveSmartMotorB, LeftDriveSmartMotorC); - -motor RightDriveSmartMotorA = motor(PORT2, ratio6_1, false); -motor RightDriveSmartMotorB = motor(PORT4, ratio6_1, true); -motor RightDriveSmartMotorC = motor(PORT6, ratio6_1, true); -motor_group RightDriveSmart = motor_group(RightDriveSmartMotorA, RightDriveSmartMotorB, RightDriveSmartMotorC); - +motor frontMogo = motor(PORT20, ratio36_1, true); +motor ringLift = motor(PORT11, ratio6_1, false); +motor LeftDriveSmartA = motor(PORT1, ratio6_1, true); +motor LeftDriveSmartB = motor(PORT3, ratio6_1, false); +motor LeftDriveSmartC = motor(PORT9, ratio6_1, false); +motor RightDriveSmartA = motor(PORT2, ratio6_1, false); +motor RightDriveSmartB = motor(PORT4, ratio6_1, true); +motor RightDriveSmartC = motor(PORT6, ratio6_1, true); digital_out rearHook = digital_out(Brain.ThreeWirePort.G); digital_out autonHook = digital_out(Brain.ThreeWirePort.A);