|
@ -166,9 +166,9 @@ public class A8kCanBusService { |
|
|
return packet.getContentI32(0); |
|
|
return packet.getContentI32(0); |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
public int moduleGetError(MId id) throws HardwareException { |
|
|
|
|
|
|
|
|
public A8kEcode moduleGetError(MId id) throws HardwareException { |
|
|
var packet = callcmd(id.toInt(), CmdId.module_get_error.toInt()); |
|
|
var packet = callcmd(id.toInt(), CmdId.module_get_error.toInt()); |
|
|
return packet.getContentI32(0); |
|
|
|
|
|
|
|
|
return A8kEcode.valueOf(packet.getContentI32(0)); |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
public void moduleClearError(MId id) throws HardwareException { |
|
|
public void moduleClearError(MId id) throws HardwareException { |
|
@ -330,21 +330,21 @@ public class A8kCanBusService { |
|
|
|
|
|
|
|
|
public boolean getIOState(IOId ioid) throws HardwareException { |
|
|
public boolean getIOState(IOId ioid) throws HardwareException { |
|
|
if (ioid.mtype == ModuleType.kboard) { |
|
|
if (ioid.mtype == ModuleType.kboard) { |
|
|
return callcmd(ioid.mid, CmdId.extboard_read_inio.toInt(), ioid.ioIndex).getContentI32(0) != 0; |
|
|
|
|
|
|
|
|
return callcmd(ioid.mid.toInt(), CmdId.extboard_read_inio.toInt(), ioid.ioIndex).getContentI32(0) != 0; |
|
|
} else if (ioid.mtype == ModuleType.ktmc_step_motor) { |
|
|
} else if (ioid.mtype == ModuleType.ktmc_step_motor) { |
|
|
return callcmd(ioid.mid, CmdId.step_motor_read_io_state.toInt(), ioid.ioIndex).getContentI32(0) != 0; |
|
|
|
|
|
|
|
|
return callcmd(ioid.mid.toInt(), CmdId.step_motor_read_io_state.toInt(), ioid.ioIndex).getContentI32(0) != 0; |
|
|
} else if (ioid.mtype == ModuleType.kmini_servo_motor_module) { |
|
|
} else if (ioid.mtype == ModuleType.kmini_servo_motor_module) { |
|
|
return callcmd(ioid.mid, CmdId.mini_servo_read_io_state.toInt(), ioid.ioIndex).getContentI32(0) != 0; |
|
|
|
|
|
|
|
|
return callcmd(ioid.mid.toInt(), CmdId.mini_servo_read_io_state.toInt(), ioid.ioIndex).getContentI32(0) != 0; |
|
|
} else { |
|
|
} else { |
|
|
throw new HardwareException(ioid.mid, A8kEcode.IllegalOperation.toInt()); |
|
|
|
|
|
|
|
|
throw new HardwareException(ioid.mid, A8kEcode.IllegalOperation); |
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
public void setIOState(IOId ioid, boolean val) throws HardwareException { |
|
|
public void setIOState(IOId ioid, boolean val) throws HardwareException { |
|
|
if (ioid.mtype == ModuleType.kboard) { |
|
|
if (ioid.mtype == ModuleType.kboard) { |
|
|
callcmd(ioid.mid, CmdId.extboard_write_outio.toInt(), ioid.ioIndex, val ? 1 : 0); |
|
|
|
|
|
|
|
|
callcmd(ioid.mid.toInt(), CmdId.extboard_write_outio.toInt(), ioid.ioIndex, val ? 1 : 0); |
|
|
} else { |
|
|
} else { |
|
|
throw new HardwareException(ioid.mid, A8kEcode.IllegalOperation.toInt()); |
|
|
|
|
|
|
|
|
throw new HardwareException(ioid.mid, A8kEcode.IllegalOperation); |
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
@ -356,13 +356,13 @@ public class A8kCanBusService { |
|
|
if (status == ModuleStatus.IDLE) { |
|
|
if (status == ModuleStatus.IDLE) { |
|
|
break; |
|
|
break; |
|
|
} else if (status == ModuleStatus.ERROR) { |
|
|
} else if (status == ModuleStatus.ERROR) { |
|
|
throw new HardwareException(mid.toInt(), moduleGetError(mid)); |
|
|
|
|
|
|
|
|
throw new HardwareException(mid, moduleGetError(mid)); |
|
|
} |
|
|
} |
|
|
} catch (HardwareException ignored) { |
|
|
} catch (HardwareException ignored) { |
|
|
} |
|
|
} |
|
|
long now = System.currentTimeMillis(); |
|
|
long now = System.currentTimeMillis(); |
|
|
if (now - startedAt > acitionOvertime) { |
|
|
if (now - startedAt > acitionOvertime) { |
|
|
throw new HardwareException(mid.toInt(), A8kEcode.Overtime.toInt()); |
|
|
|
|
|
|
|
|
throw new HardwareException(mid, A8kEcode.Overtime); |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
Thread.sleep(100); |
|
|
Thread.sleep(100); |
|
@ -378,13 +378,13 @@ public class A8kCanBusService { |
|
|
if (status == ModuleStatus.IDLE) { |
|
|
if (status == ModuleStatus.IDLE) { |
|
|
break; |
|
|
break; |
|
|
} else if (status == ModuleStatus.ERROR) { |
|
|
} else if (status == ModuleStatus.ERROR) { |
|
|
throw new HardwareException(mid.toInt(), moduleGetError(mid)); |
|
|
|
|
|
|
|
|
throw new HardwareException(mid, moduleGetError(mid)); |
|
|
} |
|
|
} |
|
|
} catch (HardwareException ignored) { |
|
|
} catch (HardwareException ignored) { |
|
|
} |
|
|
} |
|
|
long now = System.currentTimeMillis(); |
|
|
long now = System.currentTimeMillis(); |
|
|
if (now - startedAt > acitionOvertime) { |
|
|
if (now - startedAt > acitionOvertime) { |
|
|
throw new HardwareException(mid.toInt(), A8kEcode.Overtime.toInt()); |
|
|
|
|
|
|
|
|
throw new HardwareException(mid, A8kEcode.Overtime); |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
Thread.sleep(100); |
|
|
Thread.sleep(100); |
|
@ -455,7 +455,7 @@ public class A8kCanBusService { |
|
|
try { |
|
|
try { |
|
|
return this.sendCmd(pack, A8kPacket.CMD_OVERTIME); |
|
|
return this.sendCmd(pack, A8kPacket.CMD_OVERTIME); |
|
|
} catch (HardwareException e) { |
|
|
} catch (HardwareException e) { |
|
|
if (e.getErrorCode() != A8kEcode.Overtime.toInt()) |
|
|
|
|
|
|
|
|
if (e.getErrorCode() != A8kEcode.Overtime) |
|
|
; |
|
|
; |
|
|
{ |
|
|
{ |
|
|
throw e; |
|
|
throw e; |
|
@ -483,7 +483,7 @@ public class A8kCanBusService { |
|
|
|
|
|
|
|
|
long now = System.currentTimeMillis(); |
|
|
long now = System.currentTimeMillis(); |
|
|
if (now - startedAt > acitionOvertime) { |
|
|
if (now - startedAt > acitionOvertime) { |
|
|
throw new HardwareException(pack.getModuleId(), A8kEcode.Overtime.toInt()); |
|
|
|
|
|
|
|
|
throw new HardwareException(MId.valueOf(pack.getModuleId()), A8kEcode.Overtime); |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
Thread.sleep(100); |
|
|
Thread.sleep(100); |
|
@ -510,15 +510,15 @@ public class A8kCanBusService { |
|
|
receipt = receiptQueue.poll(overtime, TimeUnit.MILLISECONDS); |
|
|
receipt = receiptQueue.poll(overtime, TimeUnit.MILLISECONDS); |
|
|
} catch (InterruptedException e) { |
|
|
} catch (InterruptedException e) { |
|
|
isWaitingReceipt = false; |
|
|
isWaitingReceipt = false; |
|
|
throw new HardwareException(pack.getModuleId(), A8kEcode.Overtime.toInt()); |
|
|
|
|
|
|
|
|
throw new HardwareException(MId.valueOf(pack.getModuleId()), A8kEcode.Overtime); |
|
|
} |
|
|
} |
|
|
isWaitingReceipt = false; |
|
|
isWaitingReceipt = false; |
|
|
if (receipt == null) { |
|
|
if (receipt == null) { |
|
|
throw new HardwareException(pack.getModuleId(), A8kEcode.Overtime.toInt()); |
|
|
|
|
|
|
|
|
throw new HardwareException(MId.valueOf(pack.getModuleId()), A8kEcode.Overtime); |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
if (receipt.getPacketType() == A8kPacket.PACKET_TYPE_ERROR_ACK) { |
|
|
if (receipt.getPacketType() == A8kPacket.PACKET_TYPE_ERROR_ACK) { |
|
|
throw new HardwareException(pack.getModuleId(), receipt.getContentI32(0)); |
|
|
|
|
|
|
|
|
throw new HardwareException(MId.valueOf(pack.getModuleId()), A8kEcode.valueOf(receipt.getContentI32(0))); |
|
|
} |
|
|
} |
|
|
try { |
|
|
try { |
|
|
Thread.sleep(10); |
|
|
Thread.sleep(10); |
|
|