Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Drive improvements #14

Merged
merged 2 commits into from
Feb 8, 2023
Merged

Drive improvements #14

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c67867b
no fucking clue tbh
smaldrich Feb 7, 2023
0eccadb
no fucking clue tbh
smaldrich Feb 8, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 17 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/functions/PIDFunctions.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,23 @@ public static float updatePIDAngular(PIDData d, float pos, float dt){
return out;
}

public static float updatePID(PIDData d, float pos, float dt){

// calculate the error in degrees
float error = d.target - pos;

// rate of change of the error
float derivative = (error - d.prevErr) * dt;

// sum of all error over time
d.iSum += error * dt;

float out = (d.Kp * error) + (d.Ki * d.iSum) + (d.Kd * derivative);

d.prevErr = error;

return out;
}


// V these degree symbols are cool
Expand Down
2 changes: 2 additions & 0 deletions .../src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/ConceptVuforiaNavigation.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XZY;
import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC;

import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

Expand Down Expand Up @@ -42,6 +43,7 @@
import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XZY;
import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC;

@Disabled
public class ConceptVuforiaNavigation {
/* Copyright (c) 2019 FIRST. All rights reserved.
*
Expand Down
2 changes: 2 additions & 0 deletions ...ain/java/org/firstinspires/ftc/teamcode/opModes/autos/ObjectDetectionTeamMadeObjects.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
package org.firstinspires.ftc.teamcode.opModes.autos;

import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;

import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
Expand All @@ -20,6 +21,7 @@
import java.util.List;


@Disabled
@com.qualcomm.robotcore.eventloop.opmode.Autonomous(name="MechAuto CV Parking Custom", group="Autos")
public class ObjectDetectionTeamMadeObjects extends LinearOpMode {

Expand Down
2 changes: 2 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/OpenCVWithImages.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@


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

import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
Expand Down Expand Up @@ -30,6 +31,7 @@

@Autonomous(name="OpenCV and Feild", group="Autos")

@Disabled
public class OpenCVWithImages extends LinearOpMode
{
OpenCvCamera camera;
Expand Down
4 changes: 2 additions & 2 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/autoScoring.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XZY;
import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.HardwareMap;
Expand Down Expand Up @@ -33,7 +34,7 @@
import java.util.ArrayList;
import java.util.List;

@TeleOp(name="XAutoScoring", group ="Autos")
@Autonomous(name="XAutoScoring")
public class autoScoring extends LinearOpMode {


Expand Down Expand Up @@ -183,7 +184,6 @@ public class autoScoring extends LinearOpMode {
float x = pos.get(0);
float y = pos.get(1);

//NTS: run starting w/ the bot facing the wall!
V2f rel = new V2f(x - (-1232), y - (-1984));
TelemetryData r = new TelemetryData("Relative");
r.addChild("x", rel.x);
Expand Down
2 changes: 2 additions & 0 deletions ...de/src/main/java/org/firstinspires/ftc/teamcode/opModes/autos/givenExampleCodeOpenCV.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
package org.firstinspires.ftc.teamcode.opModes.autos;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

Expand All @@ -36,6 +37,7 @@
import java.util.ArrayList;


@Disabled
@com.qualcomm.robotcore.eventloop.opmode.Autonomous(name="Given Open CV", group="Autos")
public class givenExampleCodeOpenCV extends LinearOpMode
{
Expand Down
53 changes: 53 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/demos/PIDArmDemo.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,53 @@
package org.firstinspires.ftc.teamcode.opModes.demos;

import androidx.annotation.NonNull;

import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;

import org.firstinspires.ftc.teamcode.functions.NavFunctions;
import org.firstinspires.ftc.teamcode.functions.PIDFunctions;
import org.firstinspires.ftc.teamcode.functions.TelemetryFunctions;
import org.firstinspires.ftc.teamcode.util.Constants;
import org.firstinspires.ftc.teamcode.util.Data.ArmData;
import org.firstinspires.ftc.teamcode.util.Data.DriveData;
import org.firstinspires.ftc.teamcode.util.Data.NavData;
import org.firstinspires.ftc.teamcode.util.Data.PIDData;
import org.firstinspires.ftc.teamcode.util.Data.TelemetryData;



@com.qualcomm.robotcore.eventloop.opmode.TeleOp(name="Arm PID Demo", group="TeleOps")
@Disabled
public class PIDArmDemo extends LinearOpMode {

@Override
public void runOpMode() {

TelemetryData telemetry = new TelemetryData("State");
NavData nav = new NavData(this.hardwareMap);
PIDData armPID = new PIDData(0.01f, 0.0f, 0.0f);
@NonNull DcMotor liftMotor = this.hardwareMap.get(DcMotor.class, "lift_motor");


// Constants.initArm(arm, c);
TelemetryFunctions.sendTelemetry(this.telemetry, telemetry);


waitForStart();
while (opModeIsActive()) {

NavFunctions.updateDt(nav);
armPID.target += this.gamepad1.left_stick_y * 10 * nav.dt;

float pidOut = PIDFunctions.updatePID(armPID, liftMotor.getCurrentPosition(), (float)nav.dt);
liftMotor.setPower(pidOut);
telemetry.addChild("Arm encoder", liftMotor.getCurrentPosition());
telemetry.addChild("Target pos", armPID.target);
telemetry.addChild("pidOut", pidOut);

}
}

}
83 changes: 52 additions & 31 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/teleOp/XTeleOpAbsolute.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,12 +38,14 @@ public void runOpMode() {
TelemetryData c = new TelemetryData();
telemetry.addChild(c);
c.title = "yg;uirguhlsrtguh;sgbrhj;sreguh;srge;ioj";
Constants.initArm(arm, c);
// Constants.initArm(arm, c);
TelemetryFunctions.sendTelemetry(this.telemetry, telemetry);



waitForStart();
NavFunctions.updateHeading(nav);
drivePID.target = nav.heading;
while(opModeIsActive()){


Expand All @@ -59,50 +61,71 @@ public void runOpMode() {
NavFunctions.updateDt(nav);
NavFunctions.updateHeading(nav);



TelemetryData opModeTelemetry = new TelemetryData("OpMode");
telemetry.addChild(opModeTelemetry);


//change target angle with input
if(r.length() >= Constants.TURN_CUTOFF){
float mod = Constants.defaultXTurnSpeed
+ ((Constants.maxXTurnSpeed-Constants.defaultXTurnSpeed)*this.gamepad1.left_trigger);
drivePID.target = PIDFunctions.angleWrap(drivePID.target - (r.x * mod));





float PIDOut = 0;
{
//change target angle with input
if (r.length() >= Constants.TURN_CUTOFF) {
if (!gamepad1.left_bumper) {

float mod = Constants.defaultXTurnSpeed
+ ((Constants.maxXTurnSpeed - Constants.defaultXTurnSpeed) * this.gamepad1.left_trigger);
drivePID.target = PIDFunctions.angleWrap(drivePID.target - (r.x * mod));
} else {

drivePID.target = PIDFunctions.angleWrap(r.getAngleDeg() - 90);
}


}
PIDOut = PIDFunctions.updatePIDAngular(
drivePID,
nav.heading,
(float) nav.dt);

opModeTelemetry.addChild(new TelemetryData("Angle", nav.heading));
opModeTelemetry.addChild(new TelemetryData("PID power", PIDOut));
opModeTelemetry.addChild(new TelemetryData("Target Angle", drivePID.target));
}
opModeTelemetry.addChild(new TelemetryData("Target Angle", drivePID.target));




float PIDOut = PIDFunctions.updatePIDAngular(
drivePID,
nav.heading,
(float)nav.dt);
opModeTelemetry.addChild(new TelemetryData("Angle", nav.heading));
opModeTelemetry.addChild(new TelemetryData("PID power", PIDOut));



{

V2f driveCmd = l;
V2f dpad = new V2f(
(gamepad1.dpad_right?1:0) - (gamepad1.dpad_left?1:0),
(gamepad1.dpad_up?1:0) - (gamepad1.dpad_down?1:0)
);
if(dpad.length() > 0) {
driveCmd = dpad;
}

//multiplier for drive speed.
float mod = Constants.defaultXDriveSpeed
+ ((1-Constants.defaultXDriveSpeed)*this.gamepad1.right_trigger);
//float mod = 1.0f;
telemetry.addChild("Percent", mod);
V2f in = new V2f(
l.x * mod,
l.y * mod);
in = in.rotated(-nav.heading);
opModeTelemetry.addChild(new TelemetryData("Drive", in));
//multiplier for drive speed.
float mod = Constants.defaultXDriveSpeed
+ ((1-Constants.defaultXDriveSpeed)*this.gamepad1.right_trigger);


DriveFunctions.setPower(
drive,
in,
PIDOut);
V2f in = new V2f( driveCmd.x * mod, driveCmd.y * mod);
in = in.rotated(-nav.heading);

DriveFunctions.setPower(
drive,
in,
PIDOut);
}



Expand All @@ -129,8 +152,6 @@ public void runOpMode() {





arm.liftMotor.setPower(liftSpeed);
telemetry.addChild("Lift pos", pos);
telemetry.addChild("Lift speed", liftSpeed);
Expand Down