TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-15 16:52:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fixed all motor problems, need to test reversal of the 2.

Browse Source
This commit is contained in:
NerdyBirdy460
2023-10-21 12:02:37 -05:00
parent 10fe4324e4
commit 21a1df012a
3 changed files with 67 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ProtoBotEngineSodi.java
View File

@@ -7,14 +7,14 @@ import org.timecrafters.CenterStage.Autonomous.States.ProtoBotStateSodi;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "Autonomous (Sodi)", group = "PROTOTYPE")
@Autonomous(name = "Autonomous- Sodi", group = "Simple Test")
public class ProtoBotEngineSodi extends CyberarmEngine {
private ProtoBotSodi robot;
@Override
public void setup() {
this.robot = new ProtoBotSodi("Hello World");
this.robot = new ProtoBotSodi("Autonomous - Sodi");
this.robot.setup();
addState(new ProtoBotStateSodi());
addState(new ProtoBotStateSodi(robot));
}
}

53
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java
View File

@@ -11,12 +11,13 @@ import org.timecrafters.CenterStage.Common.ProtoBotSodi;
public class ProtoBotStateSodi extends CyberarmState {
ProtoBotSodi robot;
private double avgVelocity, avgDrivePower;
public ProtoBotStateSodi() {
private long lastTimeChecked;
public ProtoBotStateSodi(ProtoBotSodi robot) {
this.robot = robot;
}
public void telemetry() {
engine.telemetry.addData("Avg Drive Velocity", avgVelocity);
engine.telemetry.addData("Avg Drive Power", avgDrivePower);
// engine.telemetry.addData("Avg Drive Velocity", avgVelocity);
// engine.telemetry.addData("Avg Drive Power", avgDrivePower);
engine.telemetry.addData("Front Left Velocity", robot.flDrive.getVelocity());
engine.telemetry.addData("Front Right Velocity", robot.frDrive.getVelocity());
engine.telemetry.addData("Back Left Velocity", robot.blDrive.getVelocity());
@@ -28,15 +29,15 @@ public class ProtoBotStateSodi extends CyberarmState {
}
public double getAvgDrivePower() {
avgDrivePower = (robot.flDrive.motor.getPower() + robot.frDrive.motor.getPower() + robot.blDrive.motor.getPower() + robot.brDrive.motor.getPower())/4;
return avgDrivePower;
}
// public double getAvgDrivePower() {
// avgDrivePower = (robot.flDrive.motor.getPower() + robot.frDrive.motor.getPower() + robot.blDrive.motor.getPower() + robot.brDrive.motor.getPower())/4;
// return avgDrivePower;
// }
public double getAvgVelocity() {
avgVelocity = (robot.flDrive.getVelocity() + robot.frDrive.getVelocity() + robot.blDrive.getVelocity() + robot.brDrive.getVelocity())/4;
return avgVelocity;
}
// public double getAvgVelocity() {
// avgVelocity = (robot.flDrive.getVelocity() + robot.frDrive.getVelocity() + robot.blDrive.getVelocity() + robot.brDrive.getVelocity())/4;
// return avgVelocity;
// }
@Override
public void init() {
@@ -46,46 +47,52 @@ public class ProtoBotStateSodi extends CyberarmState {
robot.brDrive.motor.setPower(0);
robot.liftMotor.motor.setPower(0);
// robot.fang.setPosition(0);
// robot.jaw.setPosition(0);
// robot.neck.setPosition(0);
// robot.shoulder.setPosition(0);
// robot.wrist.setPosition(0);
// robot.hand.setPosition(0);
robot.grabJaw.setPosition(0);
robot.grabElbow.setPosition(0);
robot.grabShoulder.setPosition(0);
robot.dropShoulder.setPosition(0);
robot.dropElbow.setPosition(0);
robot.dropJaw.setPosition(0);
lastTimeChecked = System.currentTimeMillis();
}
@Override
public void exec() {
if (System.currentTimeMillis() >= 500 && System.currentTimeMillis() < 2500) {
if (System.currentTimeMillis() - lastTimeChecked >= 500 && System.currentTimeMillis() - lastTimeChecked < 2500) {
robot.flDrive.motor.setPower(0.5);
robot.frDrive.motor.setPower(0.5);
robot.blDrive.motor.setPower(0.5);
robot.brDrive.motor.setPower(0.5);
robot.liftMotor.motor.setPower(0.5);
} else if (System.currentTimeMillis() >= 2500 && System.currentTimeMillis() < 3500) {
} else if (System.currentTimeMillis() - lastTimeChecked >= 2500 && System.currentTimeMillis() - lastTimeChecked < 4500) {
robot.flDrive.motor.setPower(-0.5);
robot.frDrive.motor.setPower(-0.5);
robot.blDrive.motor.setPower(-0.5);
robot.brDrive.motor.setPower(-0.5);
robot.liftMotor.motor.setPower(-0.5);
} else if (System.currentTimeMillis() >= 3500 && System.currentTimeMillis() < 4500) {
} else if (System.currentTimeMillis() - lastTimeChecked >= 4500 && System.currentTimeMillis() - lastTimeChecked < 6500) {
robot.flDrive.motor.setPower(0.5);
robot.frDrive.motor.setPower(0.5);
robot.blDrive.motor.setPower(-0.5);
robot.brDrive.motor.setPower(-0.5);
} else if (System.currentTimeMillis() >= 4500 && System.currentTimeMillis() < 5500) {
robot.liftMotor.motor.setPower(0);
} else if (System.currentTimeMillis() - lastTimeChecked >= 6500 && System.currentTimeMillis() - lastTimeChecked < 8500) {
robot.flDrive.motor.setPower(-0.5);
robot.frDrive.motor.setPower(-0.5);
robot.blDrive.motor.setPower(0.5);
robot.brDrive.motor.setPower(0.5);
} else {
robot.liftMotor.motor.setPower(0);
} else if (System.currentTimeMillis() - lastTimeChecked >= 8600){
robot.flDrive.motor.setPower(0);
robot.frDrive.motor.setPower(0);
robot.blDrive.motor.setPower(0);
robot.brDrive.motor.setPower(0);
robot.liftMotor.motor.setPower(0);
setHasFinished(true);
}
}
}

60
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java
View File

@@ -1,5 +1,8 @@
package org.timecrafters.CenterStage.Common;
import static com.qualcomm.robotcore.hardware.DcMotorSimple.Direction.FORWARD;
import static com.qualcomm.robotcore.hardware.DcMotorSimple.Direction.REVERSE;
import org.timecrafters.Library.Robot;
import com.arcrobotics.ftclib.drivebase.HDrive;
import com.arcrobotics.ftclib.hardware.RevIMU;
@@ -23,8 +26,7 @@ public class ProtoBotSodi extends Robot {
public HardwareMap hardwareMap;
public MotorEx flDrive, frDrive, blDrive, brDrive, liftMotor;
public Servo fang, jaw, neck, shoulder, wrist, hand;
public ProtoBotSodi robot;
public Servo grabJaw, grabElbow, grabShoulder, dropShoulder, dropElbow, dropJaw;
private String string;
private CyberarmEngine engine;
@@ -40,33 +42,39 @@ public class ProtoBotSodi extends Robot {
TimeCraftersConfiguration configuration = new TimeCraftersConfiguration("Robbie");
//Motors
//MOTORS
frDrive = new MotorEx(hardwareMap, "frontRight");
flDrive = new MotorEx(hardwareMap, "frontLeft");
brDrive = new MotorEx(hardwareMap, "backRight");
blDrive = new MotorEx(hardwareMap, "backLeft");
//Motors
frDrive = new MotorEx(hardwareMap, "FrontRight");
flDrive = new MotorEx(hardwareMap, "FrontLeft");
brDrive = new MotorEx(hardwareMap, "BackRight");
blDrive = new MotorEx(hardwareMap, "BackLeft");
liftMotor = new MotorEx(hardwareMap, "Lift");
robot.flDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.frDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.blDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.brDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.liftMotor.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
flDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
frDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
blDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
brDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
liftMotor.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
//Servos
robot.jaw.setDirection(Servo.Direction.FORWARD);
robot.fang.setDirection(Servo.Direction.FORWARD);
robot.neck.setDirection(Servo.Direction.FORWARD);
robot.shoulder.setDirection(Servo.Direction.FORWARD);
robot.wrist.setDirection(Servo.Direction.FORWARD);
robot.hand.setDirection(Servo.Direction.FORWARD);
flDrive.motor.setDirection(FORWARD);
frDrive.motor.setDirection(REVERSE);
blDrive.motor.setDirection(FORWARD);
brDrive.motor.setDirection(REVERSE);
//Servos
grabJaw = hardwareMap.servo.get("GrabJaw");
grabElbow = hardwareMap.servo.get("GrabElbow");
grabShoulder = hardwareMap.servo.get("GrabShoulder");
dropShoulder = hardwareMap.servo.get("DropShoulder");
dropElbow = hardwareMap.servo.get("DropElbow");
dropJaw = hardwareMap.servo.get("DropJaw");
grabElbow.setDirection(Servo.Direction.FORWARD);
grabJaw.setDirection(Servo.Direction.FORWARD);
grabShoulder.setDirection(Servo.Direction.FORWARD);
dropShoulder.setDirection(Servo.Direction.FORWARD);
dropElbow.setDirection(Servo.Direction.FORWARD);
dropJaw.setDirection(Servo.Direction.FORWARD);
// fang = hardwareMap.servo.get("Fang");
// jaw = hardwareMap.servo.get("Jaw");
// neck = hardwareMap.servo.get("Neck");
// shoulder = hardwareMap.servo.get("Shoulder");
// wrist = hardwareMap.servo.get("Wrist");
// hand = hardwareMap.servo.get("Hand");
}
}