|
|
@ -15,13 +15,11 @@ import java.util.List; |
|
|
|
|
|
|
|
import lombok.extern.slf4j.Slf4j; |
|
|
|
|
|
|
|
@Slf4j |
|
|
|
public class ProfilometerRecorder { |
|
|
|
// |
|
|
|
// 状态 |
|
|
|
// |
|
|
|
|
|
|
|
|
|
|
|
List<XYPoint> dataCache1; //曲线缓存 |
|
|
|
List<XYPoint> dataCache2; //曲线缓存 |
|
|
|
|
|
|
@ -30,6 +28,7 @@ public class ProfilometerRecorder { |
|
|
|
|
|
|
|
|
|
|
|
MeasureSide firstSide = MeasureSide.LEFT; //第一条曲线的测量方向 |
|
|
|
double firstSideXVal = 0; |
|
|
|
|
|
|
|
Boolean rightSideReady = false; //左侧测量完成 |
|
|
|
Boolean leftSideReady = false; //右侧测量完成 |
|
|
@ -50,7 +49,6 @@ public class ProfilometerRecorder { |
|
|
|
ProfilometerAlgoConfig cfg = new ProfilometerAlgoConfig(); |
|
|
|
DeviceProfile deviceProfile; |
|
|
|
|
|
|
|
|
|
|
|
synchronized public void initialize(ProfilometerAlgoConfig config, ProfilometerDrawerListener listener) { |
|
|
|
this.listener = listener; |
|
|
|
if (config == null) { |
|
|
@ -66,6 +64,7 @@ public class ProfilometerRecorder { |
|
|
|
dataCache1 = new ArrayList<>(); |
|
|
|
dataCache2 = new ArrayList<>(); |
|
|
|
firstSide = null; |
|
|
|
firstSideXVal = 0; |
|
|
|
rightSideReady = false; |
|
|
|
lastDisplayPoint = null; |
|
|
|
leftSideReady = false; |
|
|
@ -78,6 +77,14 @@ public class ProfilometerRecorder { |
|
|
|
resetContext(); |
|
|
|
} |
|
|
|
|
|
|
|
synchronized void loginfo(String fmt, Object... args) { |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
synchronized void logdebug(String fmt, Object... args) { |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
synchronized public void processRawEncoderData(Integer sensor1data, Integer sensor2data) { |
|
|
|
sensor1data = sensor1data & 0xFFFFF000; |
|
|
@ -94,7 +101,7 @@ public class ProfilometerRecorder { |
|
|
|
|
|
|
|
if (status == RailProfileMeasureTaskStatus.START_MEASURE) { |
|
|
|
status = RailProfileMeasureTaskStatus.WAITING_FOR_RECORD_THE_1ST_SIDE; |
|
|
|
log.info("等待记录第一条曲线"); |
|
|
|
loginfo("等待记录第一条曲线"); |
|
|
|
if (listener != null) |
|
|
|
listener.onWaitingForMeasuringThe1stSide(); |
|
|
|
} else if (status == RailProfileMeasureTaskStatus.WAITING_FOR_RECORD_THE_1ST_SIDE) { |
|
|
@ -105,7 +112,7 @@ public class ProfilometerRecorder { |
|
|
|
processDataOnRecordTheFirstSide(arm1rad, arm2rad, newPoitn); |
|
|
|
} else if (status == RailProfileMeasureTaskStatus.RECORD_THE_1ST_SIDE_FINISHED) { |
|
|
|
status = RailProfileMeasureTaskStatus.WAITING_FOR_RECORD_THE_2ND_SIDE; |
|
|
|
log.info("等待记录第二条曲线 {}", firstSide.getOpposite()); |
|
|
|
loginfo("等待记录第二条曲线 {}", firstSide.getOpposite()); |
|
|
|
if (listener != null) |
|
|
|
listener.waitingForMeasuringThe2NdSide(firstSide.getOpposite()); |
|
|
|
} else if (status == RailProfileMeasureTaskStatus.WAITING_FOR_RECORD_THE_2ND_SIDE) { |
|
|
@ -238,14 +245,16 @@ public class ProfilometerRecorder { |
|
|
|
|
|
|
|
MeasureSide nowside = rad1 < 0 ? MeasureSide.LEFT : MeasureSide.RIGHT; |
|
|
|
|
|
|
|
if (nowside.equals(MeasureSide.RIGHT)) { |
|
|
|
isInStartPos = newPoint.x <= 0 && newPoint.x >= -hight / 2.0 |
|
|
|
&& newPoint.y <= weight / 2.0 && newPoint.y >= -weight / 2.0; |
|
|
|
} else { |
|
|
|
isInStartPos = newPoint.x <= hight / 2.0 && newPoint.x >= 0 |
|
|
|
&& newPoint.y <= weight / 2.0 && newPoint.y >= -weight / 2.0; |
|
|
|
} |
|
|
|
// if (nowside.equals(MeasureSide.RIGHT)) { |
|
|
|
// isInStartPos = newPoint.x <= 0 && newPoint.x >= -hight / 2.0 |
|
|
|
// && newPoint.y <= weight / 2.0 && newPoint.y >= -weight / 2.0; |
|
|
|
// } else { |
|
|
|
// isInStartPos = newPoint.x <= hight / 2.0 && newPoint.x >= 0 |
|
|
|
// && newPoint.y <= weight / 2.0 && newPoint.y >= -weight / 2.0; |
|
|
|
// } |
|
|
|
|
|
|
|
isInStartPos = newPoint.x <= hight / 2.0 && newPoint.x >= -hight / 2.0 |
|
|
|
&& newPoint.y <= weight / 2.0 && newPoint.y >= -weight / 2.0; |
|
|
|
|
|
|
|
if (stillRefPoint == null) { |
|
|
|
stillRefPoint = newPoint; |
|
|
@ -253,7 +262,7 @@ public class ProfilometerRecorder { |
|
|
|
boolean still = (distance(stillRefPoint, newPoint)) < cfg.stillJudgeThreshold; |
|
|
|
lastpoint = newPoint; |
|
|
|
|
|
|
|
// log.info("still: {}, isInStartPos: {}, newPoint: {}, lastpoint: {}", still, isInStartPos, newPoint, lastpoint); |
|
|
|
logdebug("still: {}, isInStartPos: {}, newPoint: {}, lastpoint: {}", still, isInStartPos, newPoint, lastpoint); |
|
|
|
|
|
|
|
if (!stillFlag) { |
|
|
|
if (still) { |
|
|
@ -272,7 +281,20 @@ public class ProfilometerRecorder { |
|
|
|
//开始测量第一条曲线 |
|
|
|
status = RailProfileMeasureTaskStatus.RECORD_THE_FIRST_SIDE; |
|
|
|
firstSide = rad1 < 0 ? MeasureSide.LEFT : MeasureSide.RIGHT; |
|
|
|
log.info("开始测量第一条曲线 {}", firstSide); |
|
|
|
if (firstSide.equals(MeasureSide.LEFT)) { |
|
|
|
if (newPoint.x < 0) { |
|
|
|
firstSideXVal = newPoint.x; |
|
|
|
} else { |
|
|
|
firstSideXVal = 0; |
|
|
|
} |
|
|
|
} else { |
|
|
|
if (newPoint.x > 0) { |
|
|
|
firstSideXVal = newPoint.x; |
|
|
|
} else { |
|
|
|
firstSideXVal = 0; |
|
|
|
} |
|
|
|
} |
|
|
|
loginfo("开始测量第一条曲线 {} at", firstSide, firstSideXVal); |
|
|
|
if (listener != null) |
|
|
|
listener.onStartRecordThe1stSide(firstSide); |
|
|
|
|
|
|
@ -294,13 +316,9 @@ public class ProfilometerRecorder { |
|
|
|
XYPoint lp0 = directionJudgeCache.get(directionJudgeCache.size() - 1); |
|
|
|
XYPoint lp1 = directionJudgeCache.get(directionJudgeCache.size() - 2); |
|
|
|
XYPoint lp2 = directionJudgeCache.get(directionJudgeCache.size() - 3); |
|
|
|
|
|
|
|
double ddirec = calculateAngle(lp0, lp1, lp2); |
|
|
|
|
|
|
|
// XYPoint runDirection1 = new XYPoint(lp0.x - lp1.x, lp0.y - lp1.y); |
|
|
|
// XYPoint runDirection2 = new XYPoint(lp1.x - lp2.x, lp1.y - lp2.y); |
|
|
|
// double ddirec = runDirection1.x * runDirection2.x + runDirection1.y * runDirection2.y; |
|
|
|
log.debug("ddirec: {}, speedNow: {}", ddirec, speedNow); |
|
|
|
logdebug("ddirec: {}, speedNow: {}", ddirec, speedNow); |
|
|
|
if (ddirec > 60) { |
|
|
|
endFlag = true; |
|
|
|
} |
|
|
@ -352,7 +370,7 @@ public class ProfilometerRecorder { |
|
|
|
boolean endFlag = isEnd(rad1, rad2, newPoint); |
|
|
|
if (endFlag) { |
|
|
|
//测量完成 |
|
|
|
log.info("测量完成,第一条曲线 {}", firstSide); |
|
|
|
loginfo("测量完成,第一条曲线 {}", firstSide); |
|
|
|
status = RailProfileMeasureTaskStatus.RECORD_THE_1ST_SIDE_FINISHED; |
|
|
|
resetContext(); |
|
|
|
if (firstSide == MeasureSide.LEFT) { |
|
|
@ -379,10 +397,10 @@ public class ProfilometerRecorder { |
|
|
|
* |
|
|
|
*/ |
|
|
|
|
|
|
|
if ((leftSideReady && newPoint.x >= 0) || rightSideReady && newPoint.x <= 0) { |
|
|
|
if ((leftSideReady && newPoint.x >= firstSideXVal) || rightSideReady && newPoint.x <= firstSideXVal) { |
|
|
|
//开始测量第二条曲线 |
|
|
|
status = RailProfileMeasureTaskStatus.RECORD_THE_2ND_SIDE; |
|
|
|
log.info("开始测量第二条曲线 {}", firstSide.getOpposite()); |
|
|
|
loginfo("开始测量第二条曲线 {}", firstSide.getOpposite()); |
|
|
|
if (listener != null) |
|
|
|
listener.onStartRecordThe2NdSide(firstSide.getOpposite()); |
|
|
|
dataCache2.add(newPoint); |
|
|
@ -425,7 +443,7 @@ public class ProfilometerRecorder { |
|
|
|
boolean endFlag = isEnd(rad1, rad2, newPoint); |
|
|
|
if (endFlag) { |
|
|
|
//测量完成 |
|
|
|
log.info("测量完成,第二条曲线 {}", firstSide.getOpposite()); |
|
|
|
loginfo("测量完成,第二条曲线 {}", firstSide.getOpposite()); |
|
|
|
status = RailProfileMeasureTaskStatus.FINISHED; |
|
|
|
if (listener != null) |
|
|
|
listener.onFinished(firstSide.getOpposite()); |
|
|
|