Skip to content

Commit

Permalink
新增 HardwareDevices 以维护硬件
Browse files Browse the repository at this point in the history
并移除了 namespace
  • Loading branch information
6-BennyLi-9 committed Aug 21, 2024
1 parent 6a7c905 commit d5fb76b
Show file tree
Hide file tree
Showing 12 changed files with 100 additions and 128 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,8 @@ public void update() {
Complex angle=new Complex(Math.toDegrees(pose.heading.toDouble()));
complex=complex.minus(error.times(angle).divide(angle.magnitude()));
pose=new Pose2d(complex.toVector2d(),pose.heading);
}/**
}
/**
* @return 返回我们重写的Localizer的格式,返回参数为实际位置。
*/
@Override
Expand Down
Original file line number Diff line number Diff line change
@@ -1,72 +1,72 @@
package org.firstinspires.ftc.teamcode.Hardwares.basic;

import android.util.Log;

import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareDevice;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;

import org.firstinspires.ftc.teamcode.utils.enums.HardwareType;
import org.firstinspires.ftc.teamcode.utils.enums.HardwareDevices;

import java.util.HashMap;
import java.util.Map;

/**
* @apiNote OpenCvCamera和BNU055IMU都不属于接口HardwareDevice
*/
public class HardwareSet {
public Map<HardwareType, HardwareDevice > devices;
public HardwareSet(){
public class DeviceMap {
public Map<HardwareDevices, HardwareDevice > devices;
public DeviceMap(HardwareMap hardwareMap){
devices=new HashMap<>();
for(HardwareDevices device: HardwareDevices.values()){
devices.put(device,(HardwareDevice)hardwareMap.get(device.classType,device.deviceName));
}
}

public void addDevice(HardwareDevice device, HardwareType hardwareType){
devices.put(hardwareType,device);
Log.i("HardwareSet",device.getDeviceName()+" added.");
}
public HardwareDevice getDevice(HardwareType hardwareType){
if(devices.containsKey(hardwareType)){
return devices.get(hardwareType);
public HardwareDevice getDevice(HardwareDevices hardwareDevices){
if(devices.containsKey(hardwareDevices)){
return devices.get(hardwareDevices);
}else{
throw new NullPointerException("Device Not Found:"+hardwareType.toString());
throw new NullPointerException("Device Not Found:"+ hardwareDevices.toString());
}
}
public void setDirection(HardwareType hardwareType, DcMotorSimple.Direction direction){
HardwareDevice device=getDevice(hardwareType);
public void setDirection(HardwareDevices hardwareDevices, DcMotorSimple.Direction direction){
HardwareDevice device=getDevice(hardwareDevices);
if(device instanceof DcMotorEx){
((DcMotorEx) device).setDirection(direction);
}else{
throw new RuntimeException("DcMotor's Direction Only Matches To DcMotor");
}
}
public void setPower(HardwareType hardwareType, double power){
HardwareDevice device=getDevice(hardwareType);
public void setPower(HardwareDevices hardwareDevices, double power){
HardwareDevice device=getDevice(hardwareDevices);
if(device instanceof DcMotorEx){
((DcMotorEx) device).setPower(power);
}else if(device instanceof DcMotor){
((DcMotor) device).setPower(power);
}else if(device instanceof Servos){
throw new RuntimeException("Cannot set the power of a servo at HardwareSet.class");
throw new RuntimeException("Cannot set the power of a servo at DeviceMap.class");
}
}
public void setPosition(HardwareType hardwareType, double position){
HardwareDevice device=getDevice(hardwareType);
public void setPosition(HardwareDevices hardwareDevices, double position){
HardwareDevice device=getDevice(hardwareDevices);
if(device instanceof Servo){
((Servo) device).setPosition(position);
}else{
throw new RuntimeException("Not allowed to set the position of a device witch isn't a Servo");
}
}
public double getPosition(HardwareType hardwareType){
HardwareDevice device=getDevice(hardwareType);
public double getPosition(HardwareDevices hardwareDevices){
HardwareDevice device=getDevice(hardwareDevices);
if (device instanceof Servo) {
return ((Servo) device).getPosition();
}else if(device instanceof DcMotorEx){
return ((DcMotorEx) device).getCurrentPosition();
}else if(device instanceof DcMotor){
return ((DcMotor) device).getCurrentPosition();
}else{
throw new RuntimeException("Cannot get the position of other devices at HardwareSet.class");
throw new RuntimeException("Cannot get the position of other devices at DeviceMap.class");
}
}
}
Original file line number Diff line number Diff line change
@@ -1,20 +1,15 @@
package org.firstinspires.ftc.teamcode.Hardwares.basic;

import androidx.annotation.NonNull;

import com.acmerobotics.roadrunner.Vector2d;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;

import org.firstinspires.ftc.teamcode.RuntimeOption;
import org.firstinspires.ftc.teamcode.namespace;
import org.firstinspires.ftc.teamcode.utils.Complex;
import org.firstinspires.ftc.teamcode.utils.Mathematics;
import org.firstinspires.ftc.teamcode.utils.enums.HardwareType;
import org.firstinspires.ftc.teamcode.utils.enums.HardwareDevices;

public class Motors {
public HardwareSet hardware;
public DeviceMap hardware;

//除非在手动程序中,不建议直接更改下列数值
public double LeftFrontPower,RightFrontPower,LeftRearPower,RightRearPower;
Expand All @@ -23,12 +18,8 @@ public class Motors {

private double ClassicBufPower =1, StructureBufPower =1;

public Motors(@NonNull HardwareMap hardwareMap){
this(hardwareMap,new HardwareSet());
}
public Motors(@NonNull HardwareMap hardwareMap,HardwareSet hardwareSet){
namespace namespace = new namespace();
hardware=hardwareSet;
public Motors(DeviceMap deviceMap){
hardware= deviceMap;
LeftFrontPower=0;
RightFrontPower=0;
LeftRearPower=0;
Expand All @@ -38,24 +29,15 @@ public Motors(@NonNull HardwareMap hardwareMap,HardwareSet hardwareSet){
SuspensionArmPower=0;
IntakePower=0;

hardware.addDevice(hardwareMap.get(DcMotorEx.class, namespace.Hardware.get(HardwareType.LeftFront)), HardwareType.LeftFront);
hardware.addDevice(hardwareMap.get(DcMotorEx.class, namespace.Hardware.get(HardwareType.LeftRear)), HardwareType.LeftRear);
hardware.addDevice(hardwareMap.get(DcMotorEx.class, namespace.Hardware.get(HardwareType.RightFront)), HardwareType.RightFront);
hardware.addDevice(hardwareMap.get(DcMotorEx.class, namespace.Hardware.get(HardwareType.RightRear)), HardwareType.RightRear);

hardware.addDevice(hardwareMap.get(DcMotorEx.class, namespace.Hardware.get(HardwareType.PlacementArm)), HardwareType.PlacementArm);
hardware.addDevice(hardwareMap.get(DcMotorEx.class, namespace.Hardware.get(HardwareType.SuspensionArm)), HardwareType.SuspensionArm);
hardware.addDevice(hardwareMap.get(DcMotorEx.class, namespace.Hardware.get(HardwareType.Intake)), HardwareType.Intake);

//TODO:根据实际情况修改
hardware.setDirection(HardwareType.LeftFront, DcMotorSimple.Direction.FORWARD);
hardware.setDirection(HardwareType.LeftRear, DcMotorSimple.Direction.FORWARD);
hardware.setDirection(HardwareType.RightFront, DcMotorSimple.Direction.REVERSE);
hardware.setDirection(HardwareType.RightRear, DcMotorSimple.Direction.REVERSE);
hardware.setDirection(HardwareDevices.LeftFront, DcMotorSimple.Direction.FORWARD);
hardware.setDirection(HardwareDevices.LeftRear, DcMotorSimple.Direction.FORWARD);
hardware.setDirection(HardwareDevices.RightFront, DcMotorSimple.Direction.REVERSE);
hardware.setDirection(HardwareDevices.RightRear, DcMotorSimple.Direction.REVERSE);

hardware.setDirection(HardwareType.PlacementArm, DcMotorSimple.Direction.REVERSE);
hardware.setDirection(HardwareType.Intake, DcMotorSimple.Direction.REVERSE);
hardware.setDirection(HardwareType.SuspensionArm, DcMotorSimple.Direction.FORWARD);
hardware.setDirection(HardwareDevices.PlacementArm, DcMotorSimple.Direction.REVERSE);
hardware.setDirection(HardwareDevices.Intake, DcMotorSimple.Direction.REVERSE);
hardware.setDirection(HardwareDevices.SuspensionArm, DcMotorSimple.Direction.FORWARD);
}

public void clearDriveOptions(){
Expand Down Expand Up @@ -102,15 +84,15 @@ public void updateDriveOptions(){
RightFrontPower=Mathematics.intervalClip(RightFrontPower,-1,1);
RightRearPower=Mathematics.intervalClip(RightRearPower,-1,1);
// hardware.setPower(
hardware.setPower(HardwareType.LeftFront, LeftFrontPower* ClassicBufPower);
hardware.setPower(HardwareType.LeftRear, LeftRearPower* ClassicBufPower);
hardware.setPower(HardwareType.RightFront, RightFrontPower* ClassicBufPower);
hardware.setPower(HardwareType.RightRear, RightRearPower* ClassicBufPower);
hardware.setPower(HardwareDevices.LeftFront, LeftFrontPower* ClassicBufPower);
hardware.setPower(HardwareDevices.LeftRear, LeftRearPower* ClassicBufPower);
hardware.setPower(HardwareDevices.RightFront, RightFrontPower* ClassicBufPower);
hardware.setPower(HardwareDevices.RightRear, RightRearPower* ClassicBufPower);
}
public void updateStructureOptions(){
hardware.setPower(HardwareType.PlacementArm, PlacementArmPower* StructureBufPower);
hardware.setPower(HardwareType.Intake, IntakePower* StructureBufPower);
hardware.setPower(HardwareType.SuspensionArm, SuspensionArmPower* StructureBufPower);
hardware.setPower(HardwareDevices.PlacementArm, PlacementArmPower* StructureBufPower);
hardware.setPower(HardwareDevices.Intake, IntakePower* StructureBufPower);
hardware.setPower(HardwareDevices.SuspensionArm, SuspensionArmPower* StructureBufPower);
}
/**
* @param headingDeg 必须在使用driverUsingAxisPowerInsteadOfCurrentPower时给出,其他状态下给出是无效的
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,7 @@
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
import org.firstinspires.ftc.teamcode.namespace;
import org.firstinspires.ftc.teamcode.utils.enums.HardwareType;
import org.firstinspires.ftc.teamcode.utils.enums.RobotDevices;

public class Sensors {
/** BNO055IMU 比 IMU 的稳定性更好
Expand All @@ -19,8 +18,7 @@ public class Sensors {
public double FirstAngle,XMoved,YMoved;

public Sensors(@NonNull HardwareMap hardwareMap){
org.firstinspires.ftc.teamcode.namespace namespace=new namespace();
imu=hardwareMap.get(BNO055IMU.class,namespace.Hardware.get(HardwareType.imu));
imu=hardwareMap.get(BNO055IMU.class,RobotDevices.imu.name);

BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.angleUnit= BNO055IMU.AngleUnit.DEGREES;
Expand Down
Original file line number Diff line number Diff line change
@@ -1,39 +1,25 @@
package org.firstinspires.ftc.teamcode.Hardwares.basic;

import androidx.annotation.NonNull;

import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;

import org.firstinspires.ftc.teamcode.utils.enums.HardwareType;
import org.firstinspires.ftc.teamcode.namespace;
import org.firstinspires.ftc.teamcode.utils.enums.HardwareDevices;

public class Servos {
public HardwareSet hardware;
public DeviceMap hardware;
public double FrontClipPosition,RearClipPosition;

private final static double AllowErrorPosition=0.1;
private boolean PositionInPlace;

public Servos(@NonNull HardwareMap hardwareMap){
this(hardwareMap,new HardwareSet());
}
public Servos(@NonNull HardwareMap hardwareMap,HardwareSet hardware){
namespace namespace=new namespace();
public Servos(DeviceMap hardware){
this.hardware=hardware;

this.hardware.addDevice(hardwareMap.get(Servo.class, namespace.Hardware.get(HardwareType.FrontClip)), HardwareType.FrontClip);
this.hardware.addDevice(hardwareMap.get(Servo.class, namespace.Hardware.get(HardwareType.RearClip)), HardwareType.RearClip);

PositionInPlace=false;
}

public void update(){
hardware.setPosition(HardwareType.FrontClip,FrontClipPosition);
hardware.setPosition(HardwareType.RearClip,RearClipPosition);
hardware.setPosition(HardwareDevices.FrontClip,FrontClipPosition);
hardware.setPosition(HardwareDevices.RearClip,RearClipPosition);

PositionInPlace=(Math.abs(hardware.getPosition(HardwareType.RearClip) - RearClipPosition) < AllowErrorPosition) &&
(Math.abs(hardware.getPosition(HardwareType.FrontClip)- FrontClipPosition) < AllowErrorPosition);
PositionInPlace=(Math.abs(hardware.getPosition(HardwareDevices.RearClip) - RearClipPosition) < AllowErrorPosition) &&
(Math.abs(hardware.getPosition(HardwareDevices.FrontClip)- FrontClipPosition) < AllowErrorPosition);
}

/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,6 @@

import org.firstinspires.ftc.teamcode.DriveControls.Localizers.ImuLocalizer;
import org.firstinspires.ftc.teamcode.Robot;
import org.firstinspires.ftc.teamcode.utils.Client;
import org.firstinspires.ftc.teamcode.utils.enums.driveDirection;
import org.firstinspires.ftc.teamcode.utils.enums.runningState;

Expand All @@ -21,7 +20,7 @@ public class Auto_IMUPositionTuner extends LinearOpMode {
@Override
public void runOpMode() {
double xP,yP,r;
robot=new Robot(hardwareMap, runningState.Autonomous,new Client(telemetry));
robot=new Robot(hardwareMap, runningState.Autonomous,telemetry);

while(!opModeIsActive()&&!isStopRequested()){
sleep(50);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,6 @@

import org.firstinspires.ftc.teamcode.DriveControls.Localizers.ImuLocalizer;
import org.firstinspires.ftc.teamcode.Robot;
import org.firstinspires.ftc.teamcode.utils.Client;
import org.firstinspires.ftc.teamcode.utils.enums.runningState;

/**
Expand Down Expand Up @@ -46,6 +45,6 @@ public void runOpMode() {
}
}
public void INIT(){
robot=new Robot(hardwareMap, runningState.Autonomous,new Client(telemetry));
robot=new Robot(hardwareMap, runningState.Autonomous,telemetry);
}
}
10 changes: 5 additions & 5 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@
import org.firstinspires.ftc.teamcode.Hardwares.Classic;
import org.firstinspires.ftc.teamcode.Hardwares.Structure;
import org.firstinspires.ftc.teamcode.Hardwares.Webcam;
import org.firstinspires.ftc.teamcode.Hardwares.basic.HardwareSet;
import org.firstinspires.ftc.teamcode.Hardwares.basic.DeviceMap;
import org.firstinspires.ftc.teamcode.Hardwares.basic.Motors;
import org.firstinspires.ftc.teamcode.Hardwares.basic.Sensors;
import org.firstinspires.ftc.teamcode.Hardwares.basic.Servos;
Expand All @@ -28,7 +28,7 @@
import java.util.Objects;

public class Robot {
public HardwareSet devices;
public DeviceMap devices;

public final Motors motors;
public final Sensors sensors;
Expand All @@ -51,11 +51,11 @@ public Robot(@NonNull HardwareMap hardwareMap, @NonNull runningState state, @Non
this(hardwareMap,state,new Client(telemetry));
}
public Robot(@NonNull HardwareMap hardwareMap, @NonNull runningState state, @NonNull Client client){
devices=new HardwareSet();
devices=new DeviceMap(hardwareMap);

motors=new Motors(hardwareMap,devices);
motors=new Motors(devices);
sensors=new Sensors(hardwareMap);
servos=new Servos(hardwareMap,devices);
servos=new Servos(devices);

classic=new Classic(motors,sensors);
structure=new Structure(motors,servos);
Expand Down

This file was deleted.

Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
package org.firstinspires.ftc.teamcode.utils.enums;

import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.Servo;

/**
* 仅适用于类型为<code>HardwareDevice</code> 接口下的硬件
* <p>
* 可以自动登记硬件的名字及其类型
*/
public enum HardwareDevices {
LeftFront("leftFront", DcMotorEx.class),
RightFront("eightFront", DcMotorEx.class),
LeftRear("leftRear", DcMotorEx.class),
RightRear("rightRear", DcMotorEx.class),
PlacementArm("rightLift", DcMotorEx.class),
Intake("intake", DcMotorEx.class),
FrontClip("frontClip", Servo.class),
RearClip("rearClip", Servo.class),
SuspensionArm("rack", DcMotorEx.class);
public final String deviceName;
public final Class<?> classType;

HardwareDevices(String deviceName, Class<?> classType){
this.deviceName=deviceName;
this.classType=classType;
}
}

This file was deleted.

Loading

0 comments on commit d5fb76b

Please sign in to comment.