SCDS/FtcRobotController
7
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

manual control complete, and states started again

Browse Source
This commit is contained in:
robotics2
2023-10-26 17:10:57 -07:00
parent 55eacfc391
commit 8d880cd330
5 changed files with 463 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
HARDWARE.md
View File

@ -18,10 +18,10 @@ Below are the following configurations for our robots
| motor3 | control | UltraPlanetary HD hex motor | left back leg frame | Drive back lt |
| I2C B0 | control | Color sensor V3 | Left outside leg frame | color left |
| I2C B1 | control | Color sensor V3 | Right outside leg frame | color right |
| digital01 | control | Digital device | arm frame back right | axle encoder |
| I2C B0 | expansion | 2m distance sensor | Middle Back outside leg frame | distance |
| motor0 | expansion | UltraPlanetary HD hex motor | left back arm frame | arm raise |
| motor1 | expansion | Core Hex Motor | right back arm frame | hang |
| motor3 | expansion | Digital device | arm frame back right | axle encoder |
| Servo 0 | expansion | Servo | on arm | wrist |
| Servo 1 | expansion | Servo | on arm | gripper |

186
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmStates.java Normal file
View File

@ -0,0 +1,186 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ElapsedTime;
@TeleOp( name = "ArmState")
public class ArmStates extends OpMode {
DcMotor arm;
DcMotor armThroughBoreEncoder;
static final double TICKS_TO_DEGREES = 0.07462686567;
static final double COUNTS_PER_MOTOR_REV = 537.6;
static final double DRIVE_GEAR_REDUCTION = 1.0;
static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.7 * Math.PI);
/**
* this function takes a long milliseconds parameter and sleeps
* @param millis milliseconds to sleep
*/
public void sleepmillis(long millis) {
try {
Thread.sleep(millis);
} catch (Exception e) {
}
}
/**
* stops all drive motors
*/
public void off() {
arm.setPower(0);
}
/**
* User defined init method
* This method will be called once when the INIT button is pressed.
*/
public void init() {
telemetry.addData("Status","In Init()");
telemetry.update();
arm = hardwareMap.dcMotor.get("arm raise");
armThroughBoreEncoder = hardwareMap.dcMotor.get("armThroughBoreEncoder");
}
/**
* User defined init_loop method
* This method will be called repeatedly when the INIT button is pressed.
* This method is optional. By default this method takes no action.
*/
public void init_loop(){
// Wait for the game to start (driver presses PLAY)
telemetry.addData("Status", "Initialized");
telemetry.update();
}
/**
* User defined start method.
* This method will be called once when the PLAY button is first pressed.
* This method is optional. By default this method takes not action. Example usage: Starting another thread.
*/
public void start() {
}
/**
* User defined stop method
* This method will be called when this op mode is first disabled.
* The stop method is optional. By default this method takes no action.
*/
public void stop(){
}
/**
* User defined loop method.
* This method will be called repeatedly in a loop while this op mode is running
*/
public void loop() {
if (gamepad1.right_bumper)
{
raisearm(200);
}
// int armState = 0;
// int armPos = 0;
// if(gamepad2.x)
// {
// armState = 1;
// raisearm(0 - armPos);
// armPos = 0;
// }
// if(gamepad2.y)
// {
// armState = 2;
// raisearm(1000);
// armPos = 30;
// }
// if(gamepad2.b)
// {
// armState = 2;
// raisearm(200 - armPos);
// armPos = 200;
// }
// if(gamepad2.right_bumper)
// {
// armState =+ 1;
// }
// if(gamepad2.left_bumper)
// {
// armState =- 1;
// }
// if(armState == 1)
// {
// raisearm(0 - armPos);
// armPos = 0;
// }
// if(armState == 2)
// {
// raisearm(30 - armPos);
// armPos = 30;
// }
// if(armState == 3)
// {
// raisearm(200 - armPos);
// armPos = 200;
//
// }
// Show the elapsed game time and wheel power
// telemetry.addData("Status", "Run Time: " + runtime.toString());
// telemetry.addData("Front left, Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
// telemetry.addData("Back left, Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
// telemetry.addData("Speed", speed);
telemetry.update();
}
public void raisearm(int degrees)
{
arm.setDirection(DcMotor.Direction.REVERSE);
armDriveWithEncoder(.2, degrees);
}
public void armDriveWithEncoder(double speed,
double Inches) {
int newarmTarget;
// Determine new target position, and pass to motor controller
newarmTarget = armThroughBoreEncoder.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH);
armThroughBoreEncoder.setTargetPosition(newarmTarget);
// Turn On RUN_TO_POSITION
arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// reset the timeout time and start motion.
arm.setPower(Math.abs(speed));
arm.setPower(0);
// Turn off RUN_TO_POSITION
armThroughBoreEncoder.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
}

44
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ChassisControl.java
View File

@ -92,34 +92,34 @@ public class ChassisControl extends OpMode {
* User defined loop method.
* This method will be called repeatedly in a loop while this op mode is running
*/
double num = 1;
String speed = "";
double num = 3;
String speed = "slow";
public void loop() {
frontLeft.setDirection(DcMotor.Direction.REVERSE);
backLeft.setDirection(DcMotor.Direction.REVERSE);
frontRight.setDirection(DcMotor.Direction.FORWARD);
backRight.setDirection(DcMotor.Direction.REVERSE);
if(gamepad1.a){
num = 3;
speed = "slow";
}
if(gamepad1.b){
num = 2.5;
speed = "medium";
}
if(gamepad1.y){
num = 2;
speed = "fast";
}
if(gamepad1.x){
num = 1.5;
speed = "Ludicrous";
}
if(gamepad1.x && gamepad1.y){
num = 1;
speed = "plaid";
}
// if(gamepad1.a){
// num = 3;
// speed = "slow";
// }
//// if(gamepad1.b){
//// num = 2.5;
//// speed = "medium";
//// }
//// if(gamepad1.y){
//// num = 2;
//// speed = "fast";
//// }
//// if(gamepad1.x){
//// num = 1.5;
//// speed = "Ludicrous";
//// }
//// if(gamepad1.x && gamepad1.y){
//// num = 1;
//// speed = "plaid";
//// }
axial = -gamepad1.left_stick_y/num; // Note: pushing stick forward gives negative value
lateral = gamepad1.left_stick_x/num;
yaw = gamepad1.right_stick_x/(num+0.5);

123
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FullRobotControl.java
View File

@ -20,10 +20,11 @@ public class FullRobotControl extends OpMode {
DcMotor backRight;
DcMotor frontLeft;
DcMotor backLeft;
DcMotor armMotor;
//DcMotor armMotor;
Servo gripper;
Servo wrist;
DcMotor arm;
DcMotor armThroughBoreEncoder;
public ElapsedTime runtime = new ElapsedTime();
static final double TICKS_TO_DEGREES = 0.07462686567;
static final double COUNTS_PER_MOTOR_REV = 537.6;
@ -50,6 +51,7 @@ public class FullRobotControl extends OpMode {
backRight.setPower(0);
frontLeft.setPower(0);
backLeft.setPower(0);
}
/**
* User defined init method
@ -65,11 +67,12 @@ public class FullRobotControl extends OpMode {
backRight = hardwareMap.dcMotor.get("Drive back rt");
frontLeft = hardwareMap.dcMotor.get("Drive front lt");
backLeft = hardwareMap.dcMotor.get("Drive back lt");
armMotor = hardwareMap.dcMotor.get("armMotor");
//armMotor = hardwareMap.dcMotor.get("armMotor");
gripper = hardwareMap.servo.get("gripper");
gripper.setPosition(1);
wrist = hardwareMap.servo.get("wrist");
arm = hardwareMap.dcMotor.get("arm raise");
armThroughBoreEncoder = hardwareMap.dcMotor.get("arm raise");
wrist.setPosition(1);
}
@ -108,7 +111,7 @@ public class FullRobotControl extends OpMode {
* This method will be called repeatedly in a loop while this op mode is running
*/
double num = 2;
String speed = "plaid";
String speed = "medium";
public void loop() {
frontLeft.setDirection(DcMotor.Direction.REVERSE);
@ -156,57 +159,76 @@ public class FullRobotControl extends OpMode {
leftBackPower /= max;
rightBackPower /= max;
}
if(gamepad1.dpad_down){
if(gamepad2.dpad_down){
wrist.setPosition(1);
}
if(gamepad1.dpad_up){
if(gamepad2.dpad_up){
wrist.setPosition(0.5);
}
if(gamepad1.left_bumper){
if(gamepad2.left_trigger > 0.35)
{
gripper.setPosition(0.25);
}
if(gamepad1.right_bumper){
if(gamepad2.right_trigger > 0.35){
gripper.setPosition(1);
}
int armState = 0;
if(gamepad2.x)
// int armState = 0;
// int armPos = 0;
// if(gamepad2.x)
// {
// armState = 1;
// raisearm(0 - armPos);
// armPos = 0;
// }
// if(gamepad2.y)
// {
// armState = 2;
// raisearm(1000);
// armPos = 30;
// }
// if(gamepad2.b)
// {
// armState = 2;
// raisearm(200 - armPos);
// armPos = 200;
// }
// if(gamepad2.right_bumper)
// {
// armState =+ 1;
// }
// if(gamepad2.left_bumper)
// {
// armState =- 1;
// }
// if(armState == 1)
// {
// raisearm(0 - armPos);
// armPos = 0;
// }
// if(armState == 2)
// {
// raisearm(30 - armPos);
// armPos = 30;
// }
// if(armState == 3)
// {
// raisearm(200 - armPos);
// armPos = 200;
//
// }
if(gamepad2.right_stick_y > 0)
{
armState = 1;
raisearm(0);
arm.setPower(armPower);
}
if(gamepad2.y)
if(gamepad2.left_stick_y < 0)
{
armState = 2;
raisearm(30);
}
if(gamepad2.y)
{
armState = 2;
raisearm(200);
}
if(gamepad2.right_bumper)
{
armState =+ 1;
}
if(gamepad2.left_bumper)
{
armState =- 1;
}
if(armState == 1)
{
raisearm(0);
}
if(armState == 2)
{
raisearm(30);
}
if(armState == 1)
{
raisearm(200);
arm.setPower(armPower);
}
telemetry.addData("arm is at", " %7d", armThroughBoreEncoder.getCurrentPosition());
telemetry.update();
@ -215,29 +237,30 @@ public class FullRobotControl extends OpMode {
frontRight.setPower(rightFrontPower);
backLeft.setPower(leftBackPower);
backRight.setPower(rightBackPower);
armMotor.setPower(armPower);
// Show the elapsed game time and wheel power
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Front left, Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
telemetry.addData("Back left, Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
telemetry.addData("Speed", speed);
//arm.setPower(armPower);
// Show the elapsed game time and wheel power
// telemetry.addData("Status", "Run Time: " + runtime.toString());
// telemetry.addData("Front left, Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
// telemetry.addData("Back left, Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
// telemetry.addData("Speed", speed);
telemetry.update();
}
public void raisearm(int degrees)
{
armEncoder(.2, degrees*TICKS_TO_DEGREES);
arm.setDirection(DcMotor.Direction.REVERSE);
armDriveWithEncoder(.2, degrees*TICKS_TO_DEGREES);
}
public void armEncoder(double speed,
public void armDriveWithEncoder(double speed,
double Inches) {
int newarmTarget;
// Determine new target position, and pass to motor controller
newarmTarget = arm.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH);
arm.setTargetPosition(newarmTarget);
newarmTarget = armThroughBoreEncoder.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH);
armThroughBoreEncoder.setTargetPosition(newarmTarget);
// Turn On RUN_TO_POSITION
arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
@ -251,7 +274,7 @@ public class FullRobotControl extends OpMode {
// Turn off RUN_TO_POSITION
arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
armThroughBoreEncoder.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
}

181
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/arm.java Normal file
View File

@ -0,0 +1,181 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ElapsedTime;
@TeleOp( name = "manual control")
public class arm extends OpMode {
DcMotor arm;
Servo gripper;
Servo wrist;
public double axial;
public double lateral;
public double yaw;
DcMotor frontRight;
DcMotor backRight;
DcMotor frontLeft;
DcMotor backLeft;
/**
* this function takes a long milliseconds parameter and sleeps
* @param millis milliseconds to sleep
*/
public void sleepmillis(long millis) {
try {
Thread.sleep(millis);
} catch (Exception e) {
}
}
/**
* stops all drive motors
*/
public void off() {
arm.setPower(0);
frontRight.setPower(0);
backRight.setPower(0);
frontLeft.setPower(0);
backLeft.setPower(0);
}
/**
* User defined init method
* This method will be called once when the INIT button is pressed.
*/
public void init() {
telemetry.addData("Status","In Init()");
telemetry.update();
arm = hardwareMap.dcMotor.get("arm raise");
gripper = hardwareMap.servo.get("gripper");
wrist = hardwareMap.servo.get("wrist");
frontRight = hardwareMap.dcMotor.get("Drive front rt");
backRight = hardwareMap.dcMotor.get("Drive back rt");
frontLeft = hardwareMap.dcMotor.get("Drive front lt");
backLeft = hardwareMap.dcMotor.get("Drive back lt");
}
/**
* User defined init_loop method
* This method will be called repeatedly when the INIT button is pressed.
* This method is optional. By default this method takes no action.
*/
public void init_loop(){
// Wait for the game to start (driver presses PLAY)
telemetry.addData("Status", "Initialized");
telemetry.update();
}
/**
* User defined start method.
* This method will be called once when the PLAY button is first pressed.
* This method is optional. By default this method takes not action. Example usage: Starting another thread.
*/
public void start() {
}
/**
* User defined stop method
* This method will be called when this op mode is first disabled.
* The stop method is optional. By default this method takes no action.
*/
public void stop(){
}
double num = 3;
/**
* User defined loop method.
* This method will be called repeatedly in a loop while this op mode is running
*/
public void loop() {
frontLeft.setDirection(DcMotor.Direction.REVERSE);
backLeft.setDirection(DcMotor.Direction.REVERSE);
frontRight.setDirection(DcMotor.Direction.FORWARD);
backRight.setDirection(DcMotor.Direction.REVERSE);
double armPower = gamepad2.right_stick_y/3.5;
// Normalize the values so no wheel power exceeds 100%
// This ensures that the robot maintains the desired motion.
if(gamepad2.right_stick_y != 0)
{
arm.setPower(armPower);
telemetry.addData("joystick y value", gamepad2.right_stick_y);
telemetry.update();
}
else
{
arm.setPower(0);
}
if(gamepad2.left_trigger > 0.35)
{
gripper.setPosition(0.25);
}
if(gamepad2.right_trigger > 0.35){
gripper.setPosition(1);
}
if(gamepad2.dpad_up)
{
wrist.setPosition(0.4);
}
if(gamepad2.dpad_down)
{
wrist.setPosition(1);
}
if(gamepad2.dpad_right)
{
wrist.setPosition(0);
}
axial = -gamepad1.left_stick_y/num; // Note: pushing stick forward gives negative value
lateral = gamepad1.left_stick_x/num;
yaw = gamepad1.right_stick_x/(num+0.5);
// Combine the joystick requests for each axis-motion to determine each wheel's power.
// Set up a variable for each drive wheel to save the power level for telemetry.
double leftFrontPower = axial + lateral + yaw;
double rightFrontPower = axial - lateral - yaw;
double leftBackPower = axial - lateral + yaw;
double rightBackPower = axial + lateral - yaw;
// Normalize the values so no wheel power exceeds 100%
// This ensures that the robot maintains the desired motion.
double max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower));
max = Math.max(max, Math.abs(leftBackPower));
max = Math.max(max, Math.abs(rightBackPower));
if (max > 1.0) {
leftFrontPower /= max;
rightFrontPower /= max;
leftBackPower /= max;
rightBackPower /= max;
}
frontLeft.setPower(leftFrontPower);
frontRight.setPower(rightFrontPower);
backLeft.setPower(leftBackPower);
backRight.setPower(rightBackPower);
// Show the elapsed game time and wheel power
telemetry.addData("Front left, Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
telemetry.addData("Back left, Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
telemetry.update();
}
}