TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-15 21:22:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

all servos working.

Browse Source 
strafe needs weight balance to be efficient.
This commit is contained in:
scott
2022-02-07 13:11:58 -06:00
parent be2dbd24a8
commit 619e64296f
5 changed files with 105 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

57
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/MecanumMinibot.java
View File

@@ -1,8 +1,10 @@
package org.timecrafters.minibots.cyberarm;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo;
import org.cyberarm.engine.V2.CyberarmEngine;
@@ -15,24 +17,50 @@ public class MecanumMinibot {
private CyberarmEngine engine;
public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive;
public Servo pServoRotate, pServoGrab;
public CRServo pServoArch, pServoElevate, pServoCarousel;
public MecanumMinibot(CyberarmEngine engine) {
this.engine = engine;
setupDrivetrain();
setupServos();
}
private void setupDrivetrain() {
frontLeftDrive = engine.hardwareMap.dcMotor.get("frontLeft");
frontRightDrive = engine.hardwareMap.dcMotor.get("frontRight");
backLeftDrive = engine.hardwareMap.dcMotor.get("backLeft");
backRightDrive = engine.hardwareMap.dcMotor.get("backRight");
frontLeftDrive = engine.hardwareMap.dcMotor.get("frontLeft"); //2
frontRightDrive = engine.hardwareMap.dcMotor.get("frontRight"); //3
backLeftDrive = engine.hardwareMap.dcMotor.get("backLeft"); //1
backRightDrive = engine.hardwareMap.dcMotor.get("backRight"); //0
frontLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
frontRightDrive.setDirection(DcMotorSimple.Direction.REVERSE);
backLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
backRightDrive.setDirection(DcMotorSimple.Direction.REVERSE);
}
private void setupServos(){
pServoArch = engine.hardwareMap.crservo.get("arch"); //0
pServoElevate = engine.hardwareMap.crservo.get("elevate"); //2
pServoRotate = engine.hardwareMap.servo.get("rotate"); //3
pServoGrab = engine.hardwareMap.servo.get("grab"); //4
pServoCarousel = engine.hardwareMap.crservo.get("carousel"); //5
pServoRotate.scaleRange(0.0, 1.0);
pServoGrab.scaleRange(0.0, 1.0);
pServoArch.setDirection(DcMotorSimple.Direction.FORWARD);
pServoElevate.setDirection(DcMotorSimple.Direction.REVERSE);
pServoRotate.setDirection(Servo.Direction.FORWARD);
pServoGrab.setDirection(Servo.Direction.FORWARD);
pServoCarousel.setDirection(DcMotorSimple.Direction.FORWARD);
pServoArch.setPower(0.0);
pServoElevate.setPower(0.0);
pServoRotate.setPosition(0.70);
pServoGrab.setPosition(0.9);
pServoCarousel.setPower(0.0);
}
public void driveAll(double power) {
frontLeftDrive.setPower(power);
@@ -65,19 +93,22 @@ public class MecanumMinibot {
}
public void driveStrafe(int direction, double power) {
if (direction == STRAFE_LEFT) {
frontLeftDrive.setPower(power);
frontRightDrive.setPower(-power);
double dStrafeRatioBack=1.0;
double dStrafeRatioFront=0.5;
backLeftDrive.setPower(-power);
backRightDrive.setPower(power);
if (direction == STRAFE_LEFT) {
frontLeftDrive.setPower(power*dStrafeRatioFront);
frontRightDrive.setPower(-power*dStrafeRatioFront);
backLeftDrive.setPower(-power*dStrafeRatioBack);
backRightDrive.setPower(power*dStrafeRatioBack);
} else if (direction == STRAFE_RIGHT) {
frontLeftDrive.setPower(-power);
frontRightDrive.setPower(power);
frontLeftDrive.setPower(-power*dStrafeRatioFront);
frontRightDrive.setPower(power*dStrafeRatioFront);
backLeftDrive.setPower(power);
backRightDrive.setPower(-power);
backLeftDrive.setPower(power*dStrafeRatioBack);
backRightDrive.setPower(-power*dStrafeRatioBack);
} else {
throw new RuntimeException("Invalid direction for driveStrafe()");
}

54
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumMinibotTeleOpState.java
View File

@@ -6,6 +6,7 @@ import org.cyberarm.engine.V2.CyberarmEngine;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.minibots.cyberarm.MecanumMinibot;
public class MecanumMinibotTeleOpState extends CyberarmState {
private final MecanumMinibot robot;
private float speed;
@@ -16,6 +17,8 @@ public class MecanumMinibotTeleOpState extends CyberarmState {
@Override
public void exec() {
/* ............................................................................ drive */
speed = 1.0f - engine.gamepad1.left_trigger;
if (engine.gamepad1.y) {
@@ -28,13 +31,60 @@ public class MecanumMinibotTeleOpState extends CyberarmState {
robot.driveStrafe(MecanumMinibot.STRAFE_RIGHT, speed);
} else if (engine.gamepad1.left_bumper) {
robot.driveTurn(MecanumMinibot.TURN_LEFT, speed);
robot.driveTurn(MecanumMinibot.TURN_LEFT, speed*.5);
} else if (engine.gamepad1.right_bumper) {
robot.driveTurn(MecanumMinibot.TURN_RIGHT, speed);
robot.driveTurn(MecanumMinibot.TURN_RIGHT, speed*.5);
} else {
robot.driveStop();
}
/* ............................................................................ elevator */
if(engine.gamepad1.dpad_up){
robot.pServoElevate.setPower(1.0);
}
else if(engine.gamepad1.dpad_down){
robot.pServoElevate.setPower(-1.0);
}
else{
robot.pServoElevate.setPower((0.0));
}
/* ............................................................................ arch */
if(Math.abs(engine.gamepad1.left_stick_y)>0.1){
robot.pServoArch.setPower(engine.gamepad1.left_stick_y);
}
else{
robot.pServoArch.setPower((0.0));
}
/* ............................................................................ rotate */
if(engine.gamepad1.dpad_right){ // up position
robot.pServoRotate.setPosition(0.0);
}
if(engine.gamepad1.dpad_left){ // down position
robot.pServoRotate.setPosition(0.70);
}
/* ............................................................................ grab */
if(engine.gamepad1.left_stick_x>0.5|| engine.gamepad1.right_stick_x<-0.5){ // in
robot.pServoGrab.setPosition(0.9);
}
if(engine.gamepad1.left_stick_x<-0.5){ // small out
robot.pServoGrab.setPosition(0.50);
}
if(engine.gamepad1.right_stick_x>0.5){ // big out
robot.pServoGrab.setPosition(0.0);
}
/* ............................................................................ carousel */
if(Math.abs(engine.gamepad1.left_trigger)>0.1){
robot.pServoCarousel.setPower(engine.gamepad1.left_trigger);
}
else if(Math.abs(engine.gamepad1.right_trigger)>0.1){
robot.pServoCarousel.setPower(-engine.gamepad1.right_trigger);
}
else{
robot.pServoCarousel.setPower((0.0));
}
}
@Override