From ab9aa43498a596448886287df6ca59b3fb01a247 Mon Sep 17 00:00:00 2001
From: sige <sige@sige.com>
Date: Sat, 13 Jul 2024 15:59:06 +0800
Subject: [PATCH] 1

---
 src/main/java/a8k/appbean/HardwareException.java   |  2 +-
 .../java/a8k/base_hardware/A8kCanBusService.java   | 70 ++++++++++++++++++----
 .../java/a8k/controller/TmpTestController.java     | 31 ++++++++++
 3 files changed, 92 insertions(+), 11 deletions(-)
 create mode 100644 src/main/java/a8k/controller/TmpTestController.java

diff --git a/src/main/java/a8k/appbean/HardwareException.java b/src/main/java/a8k/appbean/HardwareException.java
index b990021..970c518 100644
--- a/src/main/java/a8k/appbean/HardwareException.java
+++ b/src/main/java/a8k/appbean/HardwareException.java
@@ -21,7 +21,7 @@ public class HardwareException extends Exception {
 		return moduleId;
 	}
 
-	int getErrorCode() {
+	public int getErrorCode() {
 		return errorCode;
 	}
 }
diff --git a/src/main/java/a8k/base_hardware/A8kCanBusService.java b/src/main/java/a8k/base_hardware/A8kCanBusService.java
index dddf881..ef8f634 100644
--- a/src/main/java/a8k/base_hardware/A8kCanBusService.java
+++ b/src/main/java/a8k/base_hardware/A8kCanBusService.java
@@ -15,6 +15,8 @@ import org.springframework.stereotype.Component;
 
 import java.net.URI;
 import java.net.URISyntaxException;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
 import java.util.Timer;
 import java.util.TimerTask;
 import java.util.concurrent.BlockingQueue;
@@ -114,28 +116,76 @@ public class A8kCanBusService {
 		}, 0, 1000);
 	}
 
-
-	//TODO
-	public A8kPacket callcmd(Integer moduleId, Integer cmdId, Integer[] params) {
-		return null;
+	private A8kPacket packParamsToPacket(Integer moduleId, Integer cmdId, Integer[] params) {
+		int subCmdId = cmdId & 0xFF;
+		int mainCmdId = (cmdId >> 8) & 0xFFFF;
+		int bufferSize = 2 + 2 + 1 + 1 + 2 + 4 * params.length; // PacketIndex - MainCmdId - SubCmdId - CmdType - ModuleId - Parameters
+		ByteBuffer buffer = ByteBuffer.allocate(bufferSize);
+		buffer.order(ByteOrder.LITTLE_ENDIAN);
+		buffer.putShort((short)0); // PacketIndex
+		buffer.putShort((short) mainCmdId); // MainCmdId
+		buffer.put((byte) subCmdId); // SubCmdId
+		buffer.put((byte) 0); // CmdType
+		buffer.putShort(moduleId.shortValue()); // ModuleId
+		for (int value : params) {
+			buffer.putInt(value);
+		}
+		return new A8kPacket(buffer.array());
 	}
 
 	//TODO
-	public void callblockcmd(Integer moduleId, Integer cmdId, Integer[] params,int acitionOvertime) {
-		//调用sendCmdAutoResend
-		// 	  通过kmodule_get_status判断指令是否完成,超时未完成则报错,则调用module_stop,同时抛出异常
-		//
-
-		return;
+	public A8kPacket callcmd(Integer moduleId, Integer cmdId, Integer[] params) throws HardwareException {
+		var packet = this.packParamsToPacket(moduleId, cmdId, params);
+		return this.sendCmd(packet, A8kPacket.CMD_OVERTIME);
 	}
+
 	//TODO
 	public  A8kPacket sendCmdAutoResend(A8kPacket pack, int overtime) throws HardwareException {
 		//调用sendCmd,
 		// 如果捕获到超时异常，则重发，最多重发三次
 		// 如果是其他异常，则直接抛出异常。
+		for ( int i=0; i<3; i++ ) {
+			try {
+				return this.sendCmd(pack, A8kPacket.CMD_OVERTIME);
+			} catch ( HardwareException e ) {
+				if ( e.getErrorCode() != Errorcode.kovertime ) {
+					throw e;
+				}
+			}
+		}
 		return null;
 	}
 
+	//TODO
+	public void callblockcmd(Integer moduleId, Integer cmdId, Integer[] params,int acitionOvertime) throws HardwareException {
+		var packet = this.packParamsToPacket(moduleId, cmdId, params);
+		this.sendCmdAutoResend(packet, A8kPacket.CMD_OVERTIME);
+
+		//调用sendCmdAutoResend
+		// 	  通过kmodule_get_status判断指令是否完成,超时未完成则报错,则调用module_stop,同时抛出异常
+		//
+
+		long startedAt = System.currentTimeMillis();
+		do {
+			var pack = this.callcmd(moduleId, 0x0104, new Integer[]{});
+			var content = pack.getCmdContent();
+			var status = content[0];
+			if ( 0 == status ) {
+				break ;
+			}
+
+			long now = System.currentTimeMillis();
+			if ( now - startedAt > acitionOvertime) {
+				throw new HardwareException(pack.getModuleId(), Errorcode.kovertime);
+			}
+
+			try {
+				Thread.sleep(10);
+			} catch (InterruptedException e) {
+				throw new RuntimeException(e);
+			}
+		} while ( true );
+	}
 
 	private   A8kPacket sendCmd(A8kPacket pack, int overtime) throws HardwareException {
 		pack.setPacketIndex(packetIndex);
diff --git a/src/main/java/a8k/controller/TmpTestController.java b/src/main/java/a8k/controller/TmpTestController.java
new file mode 100644
index 0000000..b7dd16e
--- /dev/null
+++ b/src/main/java/a8k/controller/TmpTestController.java
@@ -0,0 +1,31 @@
+package a8k.controller;
+import a8k.a8k_can_protocol.CmdId;
+import a8k.a8k_can_protocol.ModuleId;
+import a8k.appbean.HardwareException;
+import a8k.base_hardware.A8kCanBusService;
+import jakarta.annotation.Resource;
+import org.springframework.stereotype.Controller;
+import org.springframework.web.bind.annotation.GetMapping;
+import org.springframework.web.bind.annotation.ResponseBody;
+@Controller
+public class TmpTestController {
+	@Resource
+	private A8kCanBusService a8kCanBusService;
+
+	@GetMapping("/tmp-test/a8k-can-bus-service-call")
+	@ResponseBody
+	public String a8kCanBusServiceCall() throws HardwareException {
+		this.a8kCanBusService.callcmd(
+			ModuleId.ServoTubeRackTubeRotateId,
+			CmdId.kmini_servo_enable,
+			new Integer[]{1}
+		);
+		this.a8kCanBusService.callblockcmd(
+			ModuleId.MotorTubeRackFeedId,
+			CmdId.kstep_motor_easy_move_by,
+			new Integer[]{5000},
+			20000
+		);
+		return "OK";
+	}
+}