From 17eead302b9587cd9fea9f226e907e56cd2d133f Mon Sep 17 00:00:00 2001
From: sige <sige@sige.com>
Date: Mon, 22 Jul 2024 13:38:39 +0800
Subject: [PATCH] 1

---
 .../hardware/MotorTubeRackMoveCtrlService.java     | 38 ++++++++++++++++++++--
 1 file changed, 36 insertions(+), 2 deletions(-)

diff --git a/src/main/java/a8k/service/hardware/MotorTubeRackMoveCtrlService.java b/src/main/java/a8k/service/hardware/MotorTubeRackMoveCtrlService.java
index 67608bf..9f133b2 100644
--- a/src/main/java/a8k/service/hardware/MotorTubeRackMoveCtrlService.java
+++ b/src/main/java/a8k/service/hardware/MotorTubeRackMoveCtrlService.java
@@ -5,6 +5,7 @@ import a8k.appbean.HardwareException;
 import a8k.base_hardware.A8kCanBusService;
 import a8k.service.db.dao.A8kTubeInfoDao;
 import a8k.service.db.dao.MotorTubeRackMoveCtrlServiceParameterDao;
+import com.iflytop.uf.util.UfCommon;
 import jakarta.annotation.Resource;
 import org.springframework.beans.factory.annotation.Autowired;
 import org.springframework.stereotype.Component;
@@ -61,6 +62,16 @@ public class MotorTubeRackMoveCtrlService {
 			CmdId.step_motor_enable.toInt(),
 			enable ? 1 : 0
 		);
+		this.a8kCanBusService.callcmd(
+			MId.ShakeModTubeScanerClampingSV.toInt(),
+			CmdId.mini_servo_enable.toInt(),
+			enable ? 1 : 0
+		);
+		this.a8kCanBusService.callcmd(
+			MId.ShakeModTubeScanerRotateSV.toInt(),
+			CmdId.mini_servo_enable.toInt(),
+			enable ? 1 : 0
+		);
 	}
 
 	public void stop() throws HardwareException {
@@ -79,6 +90,16 @@ public class MotorTubeRackMoveCtrlService {
 			CmdId.step_motor_stop.toInt(),
 			1
 		);
+		this.a8kCanBusService.callcmd(
+			MId.ShakeModTubeScanerClampingSV.toInt(),
+			CmdId.mini_servo_stop.toInt(),
+			1
+		);
+		this.a8kCanBusService.callcmd(
+			MId.ShakeModTubeScanerRotateSV.toInt(),
+			CmdId.mini_servo_stop.toInt(),
+			1
+		);
 	}
 
 
@@ -93,6 +114,12 @@ public class MotorTubeRackMoveCtrlService {
 			CmdId.step_motor_easy_move_to_zero.toInt(),
 			params.getMoveToZeroOvertime()
 		);
+		this.a8kCanBusService.callblockcmd(
+			MId.ShakeModTubeScanerClampingSV.toInt(),
+			CmdId.mini_servo_move_to.toInt(),
+			params.getScanCodeReleasePos(),
+			params.getMoveToZeroOvertime()
+		);
 	}
 
 
@@ -207,9 +234,16 @@ public class MotorTubeRackMoveCtrlService {
 	 */
 	public String moveTubeToScanPosAndScan(String tubeType, Integer tubeIndex) throws HardwareException, InterruptedException {
 		this.moveTubeToScanPos(tubeType, tubeIndex);
-
 		this.a8kCanBusService.callcmd(MId.FeedingModScannerMod.toInt(), CmdId.code_scaner_start_scan.toInt());
-		Thread.sleep(5000);
+		this.a8kCanBusService.callcmd(MId.ShakeModTubeScanerRotateSV.toInt(),CmdId.mini_servo_rotate_with_torque.toInt(), 200);
+		for ( int i=0; i<10; i++ ) {
+			var resIsReady = this.a8kCanBusService.callcmd(MId.FeedingModScannerMod.toInt(), CmdId.code_scaner_result_is_ready.toInt());
+			if ( 1 == resIsReady.getContentI32(0) ) {
+				break ;
+			}
+			UfCommon.delay(500);
+		}
+		this.a8kCanBusService.callcmd(MId.ShakeModTubeScanerRotateSV.toInt(), CmdId.mini_servo_stop.toInt(), 1);
 		var response = this.a8kCanBusService.callcmd(MId.FeedingModScannerMod.toInt(), CmdId.code_scaner_read_scaner_result.toInt());
 		this.a8kCanBusService.callcmd(MId.FeedingModScannerMod.toInt(), CmdId.code_scaner_stop_scan.toInt());
 		var contentBytes = response.getCmdContent();