Skip to content

Commit

Permalink
Add files via upload
Browse files Browse the repository at this point in the history
  • Loading branch information
pi-this authored Sep 27, 2023
1 parent d2658a0 commit 6ba1624
Show file tree
Hide file tree
Showing 12 changed files with 761 additions and 0 deletions.
29 changes: 29 additions & 0 deletions TeamCode/build.gradle
Original file line number Diff line number Diff line change
@@ -0,0 +1,29 @@
//
// build.gradle in TeamCode
//
// Most of the definitions for building your module reside in a common, shared
// file 'build.common.gradle'. Being factored in this way makes it easier to
// integrate updates to the FTC into your code. If you really need to customize
// the build definitions, you can place those customizations in this file, but
// please think carefully as to whether such customizations are really necessary
// before doing so.


// Custom definitions may go here

// Include common definitions from above.
apply from: '../build.common.gradle'
apply from: '../build.dependencies.gradle'

android {
namespace = 'org.firstinspires.ftc.teamcode'

packagingOptions {
jniLibs.useLegacyPackaging true
}
}

dependencies {
implementation project(':FtcRobotController')
annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
}
Binary file added TeamCode/lib/OpModeAnnotationProcessor.jar
Binary file not shown.
11 changes: 11 additions & 0 deletions TeamCode/src/main/AndroidManifest.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
<?xml version="1.0" encoding="utf-8"?>

<!-- Note: the actual manifest file used in your APK merges this file with contributions
from the modules on which your app depends (such as FtcRobotController, etc).
So it won't ultimately be as empty as it might here appear to be :-) -->

<!-- The package name here determines the package for your R class and your BuildConfig class -->
<manifest
xmlns:android="http://schemas.android.com/apk/res/android">
<application/>
</manifest>
Original file line number Diff line number Diff line change
@@ -0,0 +1,77 @@
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;

@Autonomous(name="Blue, Board Side")
public class BlueBoardSide extends TargetOperations {

@Override
public void runOpMode() throws InterruptedException {
runInit();
while(opModeInInit()){runInitLoop();}
waitForStart();
runStart();
if (isStopRequested()){runStop();}
while (opModeIsActive()) {runLoop();}
visionPortal.close();
}

@Override
public void runInit() {
mapMotors("fr","fl","br","bl");
forwardMotors(false,true,false,true);
initAprilTag();
}

@Override
public void runInitLoop(){}

@Override
public void runStart() {}
@Override
public void runStop() {
stopAll();
}
@Override
public void runLoop() {

// when testing the botman's robot for a test bot make back wheels half speed (fr & fl = 1 then : br & bl = 0.5)

setAprilTagVariables();
if (gamepad1.a) {
drive(0.5,0.5, 0.25,0.25);
}
else {
driveZeroPower();
}

if (gamepad1.x) {
drive(0.5,-0.5, 0.25,-0.25);
}
else {
driveZeroPower();
}

if (gamepad1.b) {
drive(-0.5,0.5, -0.25,0.25);
}
else {
driveZeroPower();
}
output(tagId);
if (tagId == 1){
output("The First Tag!");
runStop();
}
else if (tagId == 2){
output("The Second Tag!");
runStop();
}
else if (tagId == 3){
output("The Third Tag!");
runStop();
}

}
}

Original file line number Diff line number Diff line change
@@ -0,0 +1,50 @@
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;

import java.util.List;

@Autonomous(name="Blue, Far Side")
public class BlueFarSide extends TargetOperations {

@Override
public void runOpMode() {

runInit();
while(opModeInInit()){runInitLoop();}
waitForStart();
runStart();
if (isStopRequested()){runStop();}
while (opModeIsActive()) {runLoop();}
}

@Override
public void runInit() {

}

@Override
public void runInitLoop() {

}

@Override
public void runStart() {

}

@Override
public void runLoop() {

}

@Override
public void runStop() {

}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,77 @@
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;

@TeleOp
public class FieldCentric extends TargetOperations {

@Override
public void runOpMode() {
runInit();
while(opModeInInit()){runInitLoop();}
waitForStart();
runStart();
if (isStopRequested()){runStop();}
while (opModeIsActive()) {runLoop();}
}

@Override
public void runInit() {
mapMotors("fr","fl","br","bl");
forwardMotors(false,true,false,true);
imu();
}

@Override
public void runInitLoop() {

}

@Override
public void runStart() {

}

@Override
public void runLoop() {
double y = -gamepad1.left_stick_y; // Remember, Y stick value is reversed
double x = gamepad1.left_stick_x;
double rx = gamepad1.right_stick_x;

// This button choice was made so that it is hard to hit on accident,
// it can be freely changed based on preference.
// The equivalent button is start on Xbox-style controllers.
if (gamepad1.options) {
imu.resetYaw();
}

double botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);

// Rotate the movement direction counter to the bot's rotation
double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading);
double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading);

rotX = rotX * 1.1; // Counteract imperfect strafing

// Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio,
// but only if at least one is out of the range [-1, 1]
double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1);
double frontLeftPower = (rotY + rotX + rx) / denominator;
double backLeftPower = (rotY - rotX + rx) / denominator;
double frontRightPower = (rotY - rotX - rx) / denominator;
double backRightPower = (rotY + rotX - rx) / denominator;

fl.setPower(frontLeftPower);
bl.setPower(backLeftPower);
fr.setPower(frontRightPower);
br.setPower(backRightPower);

}

@Override
public void runStop() {

}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,50 @@
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;

import java.util.List;

@Autonomous(name="Red, Board Side")
public class RedBoardSide extends TargetOperations {
@Override
public void runOpMode() {

runInit();
while(opModeInInit()){runInitLoop();}
waitForStart();
runStart();
if (isStopRequested()){runStop();}
while (opModeIsActive()) {runLoop();}
}

@Override
public void runInit() {

}

@Override
public void runInitLoop() {

}

@Override
public void runStart() {

}

@Override
public void runLoop() {

}

@Override
public void runStop() {

}

}
Original file line number Diff line number Diff line change
@@ -0,0 +1,51 @@
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;

import java.util.List;

@Autonomous(name="Red, Far Side")
public class RedFarSide extends TargetOperations {

@Override
public void runOpMode() {

runInit();
while(opModeInInit()){runInitLoop();}
waitForStart();
runStart();
if (isStopRequested()){runStop();}
while (opModeIsActive()) {runLoop();}
}

@Override
public void runInit() {

}

@Override
public void runInitLoop() {

}

@Override
public void runStart() {

}

@Override
public void runLoop() {

}

@Override
public void runStop() {

}

}
Loading

0 comments on commit 6ba1624

Please sign in to comment.