Changed matchloading skills

Made mathloading in skills Jover matchloading
RIT-VEX-U · Feb 28, 2024 · 4020855 · 4020855
1 parent 57dcd10
commit 4020855
Show file tree

Hide file tree

Showing 4 changed files with 63 additions and 25 deletions.
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
@@ -1,7 +1,7 @@
 {
 	"env": {
-		"vex_compilerPath": "${config:vexrobotics.vexcode.Cpp.Toolchain.Path}/clang/bin",
-		"vex_sdkPath": "${config:vexrobotics.vexcode.Cpp.Sdk.Home}/V5/V5_20220726_10_00_00/vexv5",
+		"vex_compilerPath": "${config:vexrobotics.vexcode.Cpp.Toolchain.Path}\\clang\\bin\\clang",
+		"vex_sdkPath": "${config:vexrobotics.vexcode.Cpp.Sdk.Home}\\V5\\V5_20220726_10_00_00\\vexv5",
 		"vex_gcc": "${vex_sdkPath}/gcc/include/c++/4.9.3",
 		"vex_sdkIncludes": [
 			"${vex_sdkPath}/clang/8.0.0/include/**",

diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -1,6 +1,6 @@
 {
 	"C_Cpp.default.systemIncludePath": [
-		"${config:vexrobotics.vexcode.Cpp.Sdk.Home}/V5/V5_20220726_10_00_00/vexv5/gcc/include/sys"
+		"${config:vexrobotics.vexcode.Cpp.Sdk.Home}\\V5\\V5_20220726_10_00_00\\vexv5\\gcc\\include\\sys"
 	],
 	"files.associations": {
 		"new": "cpp",

diff --git a/.vscode/vex_project_settings.json b/.vscode/vex_project_settings.json
@@ -1,7 +1,7 @@
 {
 	"project": {
 		"name": "Junder",
-		"slot": 1,
+		"slot": 6,
 		"description": "The cool one",
 		"platform": "V5",
 		"creationDate": "Tue, 13 Feb 2024 20:37:40 GMT",

diff --git a/src/competition/autonomous.cpp b/src/competition/autonomous.cpp
@@ -50,7 +50,7 @@ void autonomous() {
 
     cata_sys.send_command(CataSys::Command::StartDropping);
 
-    while (imu.isCalibrating() || gps_sensor.isCalibrating()) {
+    while (imu.isCalibrating() || gps_sensor.isCalibrating() || cata_sys.still_dropping()) {
         vexDelay(20);
     }
 
@@ -248,48 +248,86 @@ void newMaxSkills() {
 
         // backup into match loading
         cata_sys.IntakeFully()->withTimeout(1.0),
-        drive_sys.DriveForwardCmd(10.0, FWD, 0.25)
+        drive_sys.DriveForwardCmd(10.0, FWD, 0.5)
             ->withCancelCondition(drive_sys.DriveStalledCondition(0.2))
             ->withTimeout(1.0),
 
         // match load for 42 seconds
         new RepeatUntil(
             InOrder{
-                odom.SetPositionCmd({.x = 17.0, .y = 17.0, .rot = 225}),
+                // odom.SetPositionCmd({.x = 17.0, .y = 17.0, .rot = 225}),
 
-                cata_sys.IntakeFully(),
-                intakeToCata->withTimeout(1.0),
+                // cata_sys.IntakeFully(),
+                // intakeToCata->withTimeout(1.0),
+
+                // // cata_sys.StopIntake()->withTimeout(2.0),
+
+                // drive_sys.DriveForwardCmd(7.0, REV, 0.5),
 
-                // cata_sys.StopIntake()->withTimeout(2.0),
+                // // drive_sys.TurnToHeadingCmd()
 
-                drive_sys.DriveForwardCmd(7.0, REV, 0.5),
+                // drive_sys.TurnToHeadingCmd(190)->withTimeout(1.0),
 
-                // drive_sys.TurnToHeadingCmd()
+                // // drive_sys.DriveForwardCmd(4.0, REV, 0.3)->withTimeout(1.0),
 
-                drive_sys.TurnToHeadingCmd(190)->withTimeout(1.0),
+                // cata_sys.Fire()->withTimeout(1.5),
 
-                // drive_sys.DriveForwardCmd(4.0, REV, 0.3)->withTimeout(1.0),
+                // new DelayCommand(200),
 
-                cata_sys.Fire()->withTimeout(1.5),
+                // // cata_sys.StopFiring(),
 
-                new DelayCommand(200),
+                // // cata_sys.IntakeFully()->withTimeout(0.5),
 
-                // cata_sys.StopFiring(),
+                // // drive_sys.DriveForwardCmd(4.0, FWD, 0.3)->withTimeout(1.0),
 
-                // cata_sys.IntakeFully()->withTimeout(0.5),
+                // drive_sys.TurnToHeadingCmd(225)->withTimeout(1.0),
 
-                // drive_sys.DriveForwardCmd(4.0, FWD, 0.3)->withTimeout(1.0),
+                // drive_sys.DriveForwardCmd(10.0, FWD, 0.25)
+                //     ->withCancelCondition(drive_sys.DriveStalledCondition(0.25))
+                //     ->withTimeout(1.0),
 
-                drive_sys.TurnToHeadingCmd(225)->withTimeout(1.0),
 
-                drive_sys.DriveForwardCmd(10.0, FWD, 0.25)
-                    ->withCancelCondition(drive_sys.DriveStalledCondition(0.25))
-                    ->withTimeout(1.0),
+
+                // Matchloading!
+                cata_sys.IntakeFully(),
+                // Push against bar slowly & wait for triball to load
+                new FunctionCommand([]() {
+                    drive_sys.drive_tank(0.1, 0.1);
+                    return true;
+                }),
+
+                // Up against the wall, reset odometry
+                cata_sys.WaitForIntake()->withTimeout(2),
+                odom.SetPositionCmd({.x = 17.0, .y = 17.0, .rot = 225}),
+
+                new FunctionCommand([]() {
+                    vex::task([]() {
+                        vexDelay(600);
+                        cata_sys.send_command(CataSys::Command::StartFiring);
+                        return 0;
+                    });
+
+                    return true;
+                }),
+
+                // Drive to firing position
+                drive_sys.PurePursuitCmd(PurePursuit::Path({
+                    {.x = 17, .y = 23},
+                    {.x = 19, .y = 24},
+                    {.x = 21, .y = 24},
+                    {.x = 30, .y = 20},
+                }, 4), REV, 0.3),
+
+                drive_sys.DriveForwardCmd(10, FWD)->withTimeout(3),
+                drive_sys.TurnToHeadingCmd(225)->withTimeout(3),
+                drive_sys.DriveForwardCmd(6, FWD)->withTimeout(3),
+
+                cata_sys.IntakeFully(),
 
             },
-            new IfTimePassed(10)),
+            new IfTimePassed(42)),
 
-        // cata_sys.Fire()->withTimeout(1.0),
+        cata_sys.Fire()->withTimeout(1.0),
 
         // new DelayCommand(200),