ServoUsingMotor Impl

Author: 年迈的老秋风Windy

src/main/java/org/darbots/corebotlib/hardware/Encoder.java

@@ -5,7 +5,7 @@
 
 public interface Encoder {
     EncoderTypeInstance getEncoderType();
-    int getCurrentTick();
+    long getCurrentTick();
     Angle getCurrentAngularSpeed();
     double getCurrentTicksPerSecond();
     boolean isDirectionReversed();

src/main/java/org/darbots/corebotlib/hardware/PIDFMotor.java

@@ -8,8 +8,8 @@ public enum RunMode{
         PIDF_WITH_SPEED,
         NO_PIDF_WITH_SPEED
     }
-    int getTargetPositionTick();
-    void setTargetPositionTick(int targetPositionTick);
+    long getTargetPositionTick();
+    void setTargetPositionTick(long targetPositionTick);
     RunMode getCurrentRunMode();
     void setCurrentRunMode(RunMode mode);
     PIDFCoefficients getSpeedPIDFCoefficients();

src/main/java/org/darbots/corebotlib/hardware/PIDFMotorImpl.java

@@ -10,7 +10,7 @@
 public class PIDFMotorImpl implements PIDFMotor, AsyncDevice {
     public SimpleMotor motor;
     public Encoder encoder;
-    private int targetPosition = 0;
+    private long targetPosition = 0;
     private RunMode currentRunMode = RunMode.NO_PIDF_WITH_SPEED;
     private double currentPower = 0.0;
     private PIDFController SpeedPIDFController = new PIDFController(new PIDFCoefficients());
@@ -31,12 +31,12 @@ public PIDFMotorImpl(PIDFMotorImpl motorImpl){
         this.errorToleranceTicks = motorImpl.errorToleranceTicks;
     }
     @Override
-    public int getTargetPositionTick() {
+    public long getTargetPositionTick() {
         return this.targetPosition;
     }
 
     @Override
-    public void setTargetPositionTick(int targetPositionTick) {
+    public void setTargetPositionTick(long targetPositionTick) {
         this.targetPosition = targetPositionTick;
     }
 
@@ -103,7 +103,7 @@ public EncoderTypeInstance getEncoderType() {
     }
 
     @Override
-    public int getCurrentTick() {
+    public long getCurrentTick() {
         if(this.encoder == null){
             return 0;
         }else{
@@ -198,6 +198,12 @@ public boolean isBusy() {
 
     @Override
     public void update() {
+        if(this.motor instanceof AsyncDevice){
+            ((AsyncDevice) this.motor).update();
+        }
+        if(this.encoder instanceof AsyncDevice){
+            ((AsyncDevice) this.encoder).update();
+        }
         if(!this.isBusy()){
             this.stop();
         }
@@ -239,7 +245,7 @@ protected void applyEncoderPower(double currentTPS, double maxTPS, double power)
         this.motor.setPower(pidfPower);
     }
 
-    protected void applyPIDFToPosition(int currentPosition, int targetPosition, double currentTPS, double maxTPS, double power){
+    protected void applyPIDFToPosition(long currentPosition, long targetPosition, double currentTPS, double maxTPS, double power){
         if(this.motor == null){
             return;
         }

src/main/java/org/darbots/corebotlib/hardware/Servo.java

@@ -3,6 +3,9 @@
 import org.darbots.corebotlib.hardware.typedef.instances.ServoTypeInstance;
 
 public interface Servo {
+    public static interface ServoPositionReachedCallback{
+        void positionReached(Servo servo, double targetPosition, double actualPosition);
+    }
     /**
      * Get last set position
      * @return Last set position in range of [-1,1]

src/main/java/org/darbots/corebotlib/hardware/ServoUsingMotor.java

@@ -1,31 +1,131 @@
 package org.darbots.corebotlib.hardware;
 
+import org.darbots.corebotlib.calculations.Range;
+import org.darbots.corebotlib.hardware.typedef.instances.CRServoTypeInstance;
 import org.darbots.corebotlib.hardware.typedef.instances.ServoTypeInstance;
 
 public class ServoUsingMotor implements Servo, AsyncDevice {
-    
+    private double targetPosition;
+    private long encoderTickAtZeroPos;
+    private PIDFMotor motor;
+    private double targetPower;
+    private double maximumPosition;
+    private double minimumPosition;
+
+    public ServoUsingMotor(PIDFMotor motor, double targetPower, double minPosition, double maxPosition, double initialPosition){
+        this.motor = motor;
+        this.motor.setCurrentRunMode(PIDFMotor.RunMode.PIDF_TO_POSITION);
+        this.setTargetPower(targetPower);
+        this.minimumPosition = minPosition;
+        this.maximumPosition = maxPosition;
+        this.calibrateCurrentPosition(initialPosition);
+        this.targetPosition = initialPosition;
+    }
+
+    public ServoUsingMotor(ServoUsingMotor servoUsingMotor){
+        this.motor = servoUsingMotor.motor;
+        this.targetPosition = servoUsingMotor.targetPosition;
+        this.encoderTickAtZeroPos = servoUsingMotor.encoderTickAtZeroPos;
+        this.targetPower = servoUsingMotor.targetPower;
+        this.maximumPosition = servoUsingMotor.maximumPosition;
+        this.minimumPosition = servoUsingMotor.minimumPosition;
+    }
+
+    public double getPositionTolerance(){
+        return ((double) this.motor.getPositionErrorToleranceTicks()) / this.motor.getEncoderType().getTicksPerRev();
+    }
+
+    public void setPositionTolerance(double tolerance){
+        this.motor.setPositionErrorToleranceTicks((int) Math.round(tolerance * this.motor.getEncoderType().getTicksPerRev()));
+    }
+
+    public PIDFMotor getMotor(){
+        return this.motor;
+    }
+
+    public double getTargetPower(){
+        return this.targetPower;
+    }
+
+    public void setTargetPower(double power){
+        this.targetPower = Range.clip(Math.abs(power),0.0,1.0);
+        this.motor.setPower(this.targetPower);
+    }
+
     @Override
     public double getTargetPosition() {
-        return 0;
+        return this.targetPosition;
+    }
+
+    public void calibrateCurrentPosition(double currentPosition){
+        long currentEncoderCount = this.motor.getCurrentTick();
+        long deltaEncoderCount = Math.round(currentPosition * this.motor.getEncoderType().getTicksPerRev());
+        this.encoderTickAtZeroPos = currentEncoderCount - deltaEncoderCount;
     }
 
+    public long targetPositionToTicks(double targetPosition){
+        return Math.round(targetPosition * motor.getEncoderType().getTicksPerRev()) + encoderTickAtZeroPos;
+    }
+
+    public double encoderTicksToTargetPosition(long ticks){
+        return (ticks - encoderTickAtZeroPos) / motor.getEncoderType().getTicksPerRev();
+    }
+
+
     @Override
     public void setTargetPosition(double targetPosition) {
+        this.motor.setTargetPositionTick(this.targetPositionToTicks(targetPosition));
+        this.targetPosition = targetPosition;
+    }
 
+    public void setTargetPosition(double targetPosition, double targetPower){
+        this.setTargetPower(targetPower);
+        this.setTargetPosition(targetPosition);
     }
 
     @Override
     public ServoTypeInstance getServoType() {
-        return null;
+        return new ServoTypeInstance(
+                "ServoUsingMotor",
+                "IntegratedSensors",
+                1500,
+                2500,
+                this.getMaxAngleRange(),
+                CRServoTypeInstance.getSecondsToTurn60Deg(this.motor.getMotorType().getMaximumRPM()),
+                this.motor.getMotorType().getMinimumVoltage(),
+                this.motor.getMotorType().getMaximumVoltage()
+        );
     }
 
     @Override
     public boolean isBusy() {
-        return false;
+        return motor.isBusy();
+    }
+
+    public double getMaxAngleRange(){
+        return (this.maximumPosition - this.minimumPosition) * 360.0;
+    }
+
+    public double getMaximumPosition(){
+        return this.maximumPosition;
+    }
+
+    public void setMaximumPosition(double maximumPosition){
+        this.maximumPosition = maximumPosition;
+    }
+
+    public double getMinimumPosition(){
+        return this.minimumPosition;
+    }
+
+    public void setMinimumPosition(double minimumPosition){
+        this.minimumPosition = minimumPosition;
     }
 
     @Override
     public void update() {
-
+        if(motor instanceof AsyncDevice){
+            ((AsyncDevice) motor).update();
+        }
     }
 }