Diff: FIZ_readers/FIZ_ArriCmotion.cpp

Revision:

63:11ee40ec32bd

Parent:

62:dcb92159ac8e

--- a/FIZ_readers/FIZ_ArriCmotion.cpp	Mon Oct 25 09:04:21 2021 +0000
+++ b/FIZ_readers/FIZ_ArriCmotion.cpp	Thu Nov 11 20:18:17 2021 +0000
@@ -21,19 +21,32 @@
 void FIZ_ArriCmotion::requestCurrent(void)
 {
     _port.putc(HeaderByte);
+    _port.putc(packetSizeToSend+5); //packet length
     _port.putc('O'); 
-    _port.putc(packetSizeToSend);
+    _port.putc(0x40);//FOCUS
+    _port.putc(0);
+    _port.putc(0);
+    _port.putc(0);
+    _port.putc(0x40);//IRIS
+    _port.putc(0);
+    _port.putc(0);
+    _port.putc(0);
+    _port.putc(0x40);//ZOOM
+    _port.putc(0);
+    _port.putc(0);
+    _port.putc(0);
     // value of 0 for the first byte indicates client device with invalid data.
     // So shouldn't move the motor, only read the values. 
     // may possibly need to change this to 0x40 0x00 0x00 0x00 for each axis
     // (master but invalid data)
-    for (int i=0;i<packetSizeToSend;i++)
-      _port.putc(0);
+//    for (int i=0;i<packetSizeToSend;i++)
+//      _port.putc(0);
     
     // checksum - if all data is 0 then cs is 0. If data above changes then change this.
     _port.putc(0);
     
     _port.putc(FooterByte);
+    pc.printf("S");
 }
 
 
@@ -41,11 +54,12 @@
 {
     uint8_t dIn = _port.getc();
     inputBuffer[inputPtr] = dIn;
+    pc.printf("R");
     if (inputPtr==0) {
         if (dIn == HeaderByte) {
             inputPtr++;
         }
-    } else if (inputPtr == 1) {
+    } else if (inputPtr == 2) {
         if ((dIn == 'o')||(dIn == 'O')) { // ID 'o' = Motor status. or 'O' = motor command
             inputPtr++;
         } else { // wrong ID
@@ -58,13 +72,13 @@
         }
     } else { // waiting for data.
         if (inputPtr == 2) { // just got length
-            if (inputBuffer[2]>20) // length is too high
+            if (inputBuffer[1]>20) // length is too high
                 inputPtr=0;
             else
                 inputPtr++;
         } else {
             inputPtr++;
-            if ((inputPtr>4) && (inputPtr == (inputBuffer[2]+5))) {
+            if ((inputPtr>4) && (inputPtr == (inputBuffer[1]))) {
                 parsePacket();
                 inputPtr = 0;
             }
@@ -74,6 +88,7 @@
 
 void FIZ_ArriCmotion::parsePacket()
 {
+    pc.printf("P");
     if (inputBuffer[0] != HeaderByte)
         return;
     if (inputBuffer[1] != 'o') // only parse status