Merge branch 'master' of https://github.com/DarlingtonProgramming/DarbotsBotLibCore

Author: realquantumcookie

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

@@ -11,9 +11,11 @@ public class ServoUsingMotor implements Servo, AsyncDevice {
     private double targetPower;
     private double maximumPosition;
     private double minimumPosition;
+    public Servo.ServoPositionReachedCallback positionReachedCallback = null;
+    private boolean lastIsBusy = false;
 
     public ServoUsingMotor(PIDFMotor motor, double targetPower, double minPosition, double maxPosition, double initialPosition){
-        this.motor = motor;
+        this.setMotor(motor);
         this.motor.setCurrentRunMode(PIDFMotor.RunMode.PIDF_TO_POSITION);
         this.setTargetPower(targetPower);
         this.minimumPosition = minPosition;
@@ -23,7 +25,7 @@ public ServoUsingMotor(PIDFMotor motor, double targetPower, double minPosition,
     }
 
     public ServoUsingMotor(ServoUsingMotor servoUsingMotor){
-        this.motor = servoUsingMotor.motor;
+        this.setMotor(servoUsingMotor.motor);
         this.targetPosition = servoUsingMotor.targetPosition;
         this.encoderTickAtZeroPos = servoUsingMotor.encoderTickAtZeroPos;
         this.targetPower = servoUsingMotor.targetPower;
@@ -43,6 +45,11 @@ public PIDFMotor getMotor(){
         return this.motor;
     }
 
+    public void setMotor(PIDFMotor motor){
+        this.motor = motor;
+        this.lastIsBusy = motor.isBusy();
+    }
+
     public double getTargetPower(){
         return this.targetPower;
     }
@@ -52,6 +59,13 @@ public void setTargetPower(double power){
         this.motor.setPower(this.targetPower);
     }
 
+    public double getCurrentPosition(){
+        long currentEncoderCount = this.motor.getCurrentTick();
+        long deltaEncoderCount = currentEncoderCount - this.encoderTickAtZeroPos;
+        double currentPosition = deltaEncoderCount / this.motor.getEncoderType().getTicksPerRev();
+        return currentPosition;
+    }
+
     @Override
     public double getTargetPosition() {
         return this.targetPosition;
@@ -71,11 +85,12 @@ public double encoderTicksToTargetPosition(long ticks){
         return (ticks - encoderTickAtZeroPos) / motor.getEncoderType().getTicksPerRev();
     }
 
-
     @Override
     public void setTargetPosition(double targetPosition) {
+        targetPosition = Range.clip(targetPosition,this.minimumPosition,this.maximumPosition);
         this.motor.setTargetPositionTick(this.targetPositionToTicks(targetPosition));
         this.targetPosition = targetPosition;
+        this.lastIsBusy = true;
     }
 
     public void setTargetPosition(double targetPosition, double targetPower){
@@ -127,5 +142,15 @@ public void update() {
         if(motor instanceof AsyncDevice){
             ((AsyncDevice) motor).update();
         }
+        {
+            boolean currentIsBusy = this.motor.isBusy();
+            if (lastIsBusy && (!currentIsBusy)) {
+                if (this.positionReachedCallback != null) {
+                    this.positionReachedCallback.positionReached(this, this.targetPosition, this.getCurrentPosition());
+                    this.positionReachedCallback = null;
+                }
+            }
+            lastIsBusy = currentIsBusy;
+        }
     }
 }