Skip to content

Commit

Permalink
Merge remote-tracking branch 'origin/Train' into Train
Browse files Browse the repository at this point in the history 
# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/THEONE.java
  • Loading branch information
@ieatred40
ieatred40 committed Nov 21, 2024
2 parents bd7e37c + 2481be2 commit 887fefa
Show file tree
Hide file tree
Showing 5 changed files with 285 additions and 72 deletions.
123 changes: 98 additions & 25 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/THEONE.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,17 +7,20 @@
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;

import org.firstinspires.ftc.teamcode.util.PIDController2;

@Config
@TeleOp
public class THEONE extends LinearOpMode {


private DcMotor leftFrontMotor;
private DcMotor rightFrontMotor;
private DcMotor leftBackMotor;
private DcMotor rightBackMotor;
private DcMotor Arm;
double reference;
private DcMotor elbow;
private DcMotor viper;
private PIDController2 PIDA;

@Override
public void runOpMode() throws InterruptedException {
Expand All @@ -27,18 +30,44 @@ public void runOpMode() throws InterruptedException {
leftBackMotor = hardwareMap.get(DcMotor.class, "back_left");
rightBackMotor = hardwareMap.get(DcMotor.class, "back_right");
Arm = hardwareMap.get(DcMotor.class, "arm");
double encoderPosition;
elbow = hardwareMap.get(DcMotor.class, "elbow");
viper = hardwareMap.get(DcMotor.class, "height");
double encoderPositionA = 0;
double encoderPositionE = 0;
waitForStart();
double error;
error = 1.1;
double power = 0;
double iError = 0;
double eError = 0;
double ePower = 0;
double eIError = 0;
double ePrevError = 0;
double eDiff = 0;
double eKp = 0;
double eKi = 0;
double eKd = 0;
double Kp = 0.005;
double Ki = 0;
double Kd = 0;

double diff;
double prevError;
double armReference = 300;

PIDA = new PIDController2(armReference, Kp, Ki, Kd);
//remember what we did with classes

double eReference = 0;
double viperPower = 0;


reference = 300;
Arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
Arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
elbow.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
elbow.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

while (opModeIsActive()) {
if (gamepad1.b) {


while (gamepad1.b) {
// obtain the encoder position
Expand All @@ -54,30 +83,74 @@ else if(power<-0.7){
}
Arm.setPower(power);

telemetry.addData("arm", encoderPosition);
telemetry.addData("error", error);
telemetry.addData("stick", gamepad2.left_stick_y);
telemetry.addData("power", power);
telemetry.update();
eError = eReference - encoderPositionE;


//reference is where we want to be

eReference = 0;


// obtain the encoder position

encoderPositionE = elbow.getCurrentPosition();


eIError = eIError + eError;
//cumulative error

eDiff = eError - ePrevError;
//calculate the D


eError = eReference - encoderPositionE;
// calculate the error


ePower = eKp * eError + eKi * eIError + eKd * eDiff;


//reference is where we want to be





Arm.setPower(PIDA.update(Arm.getCurrentPosition()));
elbow.setPower(ePower);

telemetry.addData("arm", encoderPositionA);
telemetry.addData("error A", error);
telemetry.addData("reference", armReference);
telemetry.addData("power", power);
telemetry.addData("elbow", encoderPositionE);
telemetry.addData("Error E", eError);
telemetry.addData("E Power", ePower);
telemetry.update();


}
else Arm.setPower(0);

//driving

leftFrontMotor.setDirection(DcMotorSimple.Direction.REVERSE);
leftBackMotor.setDirection(DcMotorSimple.Direction.REVERSE);
double y = -gamepad1.left_stick_y; // Remember, Y stick is reversed!
double x = gamepad1.left_stick_x;
double rx = gamepad1.right_stick_x;

leftFrontMotor.setPower(y + x + rx);
leftBackMotor.setPower(y - x + rx);
rightFrontMotor.setPower(y - x - rx);
rightBackMotor.setPower(y + x - rx);


}
Arm.setPower(0);
leftFrontMotor.setDirection(DcMotorSimple.Direction.REVERSE);
leftBackMotor.setDirection(DcMotorSimple.Direction.REVERSE);
double y = -gamepad1.left_stick_y; // Remember, Y stick is reversed!
double x = gamepad1.left_stick_x;
double rx = gamepad1.right_stick_x;

leftFrontMotor.setPower(y + x + rx);
leftBackMotor.setPower(y - x + rx);
rightFrontMotor.setPower(y - x - rx);
rightBackMotor.setPower(y + x - rx);

//oops i didn't push right :P
}
}
}




//3 hours for 1 bracket... always remember to click code->reformat code
Expand Down
74 changes: 27 additions & 47 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/nickpidloop.java
View file
Original file line number Diff line number Diff line change
@@ -1,60 +1,40 @@
package org.firstinspires.ftc.teamcode.opmode;

import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;

import kotlin.reflect.KDeclarationContainer;


@Config
@TeleOp
public class nickpidloop extends LinearOpMode {

private DcMotor leftFrontMotor;
private DcMotor rightFrontMotor;
private DcMotor leftBackMotor;
private DcMotor rightBackMotor;
private DcMotor Arm;
double refrence;

public void runOpMode() throws InterruptedException {

leftFrontMotor = hardwareMap.get(DcMotor.class, "front_left");
rightFrontMotor = hardwareMap.get(DcMotor.class, "front_right");
leftBackMotor = hardwareMap.get(DcMotor.class, "back_left");
rightBackMotor = hardwareMap.get(DcMotor.class, "back_right");
Arm = hardwareMap.get(DcMotor.class, "arm");
double encoderPosition = 0;
waitForStart();
double error = 1.1;
double power = 0;
double difference = 0;
double preverror = 0;
double kp = 0;
double Ki = 0;
double kg = 0;
double k = 0;



refrence = 300;
Arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
Arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

private double reference = 250;
private double error = 0;
private double power = 0;
private double maxpower = 0.5;
private double kI = 0.1;
private double kP = 0.1;
private double kD = 0.1;

private DcMotor arm = hardwareMap.get(DcMotor.class, "arm");

public void runOpMode() throws InterruptedException {
arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
while (opModeIsActive()) {

while (gamepad1.b);

error = preverror + error;
difference = error -preverror;
preverror = error;
power = k*(error) + Ki*(error)+ kg;

}

while (gamepad1.a) {
//for encoder position
double curPosition = arm.getCurrentPosition();
//the other error + current error
double prevError = error;
//other poop that i don't understand yes
// todo learn the stuff under here
error = reference - curPosition;
double diff = error - prevError;
arm.setPower((kP * error) + (kI * error) + (kD * diff));

}
}
}


}
94 changes: 94 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/workingpidloop18284.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,94 @@
package org.firstinspires.ftc.teamcode.opmode;

import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;

@Config
@TeleOp
public class workingpidloop18284 extends LinearOpMode {


private DcMotor leftFrontMotor;
private DcMotor rightFrontMotor;
private DcMotor leftBackMotor;
private DcMotor rightBackMotor;
private DcMotor Arm;
double reference;

@Override
public void runOpMode() throws InterruptedException {

leftFrontMotor = hardwareMap.get(DcMotor.class, "front_left");
rightFrontMotor = hardwareMap.get(DcMotor.class, "front_right");
leftBackMotor = hardwareMap.get(DcMotor.class, "back_left");
rightBackMotor = hardwareMap.get(DcMotor.class, "back_right");
Arm = hardwareMap.get(DcMotor.class, "arm");
double encoderPosition;
waitForStart();
double error;
error = 1.1;
double power = 0;


reference = 300;
Arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
Arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

while (opModeIsActive()) {

while (gamepad1.b) {


// obtain the encoder position
encoderPosition = Arm.getCurrentPosition();
// calculate the error
error = reference - encoderPosition;
power = (error/250)*.7;
if(power>0.7){
power = 0.7;
}
else if(power<-0.7){
power = -0.7;
}

Arm.setPower(power);

telemetry.addData("arm", encoderPosition);
telemetry.addData("error", error);
telemetry.addData("stick", gamepad2.left_stick_y);
telemetry.addData("power", power);
telemetry.update();


}
Arm.setPower(0);
leftFrontMotor.setDirection(DcMotorSimple.Direction.REVERSE);
leftBackMotor.setDirection(DcMotorSimple.Direction.REVERSE);
double y = -gamepad1.left_stick_y; // Remember, Y stick is reversed!
double x = gamepad1.left_stick_x;
double rx = gamepad1.right_stick_x;

leftFrontMotor.setPower(y + x + rx);
leftBackMotor.setPower(y - x + rx);
rightFrontMotor.setPower(y - x - rx);
rightBackMotor.setPower(y + x - rx);

//oops i didn't push right :P
}
}
}


//3 hours for 1 bracket... always remember to click code->reformat code









1 change: 1 addition & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/PIController.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@ public PIController(double pGain, double iGain, double iWindupLimit) {
this.iWindupLimit = iWindupLimit;
}


public double update(double input) {
iSum += input;
iSum = Range.clip(iSum, -iWindupLimit, iWindupLimit);
Expand Down
Loading

0 comments on commit 887fefa

Please sign in to comment.