I messed up the merge, so pushing it over to another repo so I don't lose it. Will tidy up and remove later
Dependencies: BufferedSerial FatFileSystemCpp mbed
FIZ_readers/FIZ_ArriCmotion.cpp@85:0cc5931bb9ef, 21 months ago (annotated)
- Committer:
- JamieB
- Date:
- Thu Dec 15 06:05:30 2022 +0000
- Revision:
- 85:0cc5931bb9ef
- Parent:
- 63:11ee40ec32bd
Push to somewhere else due to merge issue
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
AndyA | 62:dcb92159ac8e | 1 | #include "FIZ_ArriCmotion.h" |
AndyA | 62:dcb92159ac8e | 2 | #include "LTCApp.h" |
AndyA | 62:dcb92159ac8e | 3 | |
AndyA | 62:dcb92159ac8e | 4 | FIZ_ArriCmotion::FIZ_ArriCmotion(const PinName Tx, const PinName Rx) : FIZReader(Tx,Rx) |
AndyA | 62:dcb92159ac8e | 5 | { |
AndyA | 62:dcb92159ac8e | 6 | inputPtr = 0; |
AndyA | 62:dcb92159ac8e | 7 | newData = false; |
AndyA | 62:dcb92159ac8e | 8 | _port.baud(38400); |
AndyA | 62:dcb92159ac8e | 9 | _port.attach(callback(this, &FIZ_ArriCmotion::OnRx)); |
AndyA | 62:dcb92159ac8e | 10 | } |
AndyA | 62:dcb92159ac8e | 11 | |
AndyA | 62:dcb92159ac8e | 12 | // expect hex data format: |
AndyA | 62:dcb92159ac8e | 13 | // 0x02 [commandID] [LEN] [len-5 bytes data] [checksum] [0x33] |
AndyA | 62:dcb92159ac8e | 14 | // checksum is xor of all data bytes. |
AndyA | 62:dcb92159ac8e | 15 | #define HeaderByte 0x02 |
AndyA | 62:dcb92159ac8e | 16 | #define FooterByte 0x33 |
AndyA | 62:dcb92159ac8e | 17 | |
AndyA | 62:dcb92159ac8e | 18 | // some ambiguity as to how large the packets should be |
AndyA | 62:dcb92159ac8e | 19 | #define packetSizeToSend 12 |
AndyA | 62:dcb92159ac8e | 20 | |
AndyA | 62:dcb92159ac8e | 21 | void FIZ_ArriCmotion::requestCurrent(void) |
AndyA | 62:dcb92159ac8e | 22 | { |
AndyA | 62:dcb92159ac8e | 23 | _port.putc(HeaderByte); |
JamieB | 63:11ee40ec32bd | 24 | _port.putc(packetSizeToSend+5); //packet length |
AndyA | 62:dcb92159ac8e | 25 | _port.putc('O'); |
JamieB | 63:11ee40ec32bd | 26 | _port.putc(0x40);//FOCUS |
JamieB | 63:11ee40ec32bd | 27 | _port.putc(0); |
JamieB | 63:11ee40ec32bd | 28 | _port.putc(0); |
JamieB | 63:11ee40ec32bd | 29 | _port.putc(0); |
JamieB | 63:11ee40ec32bd | 30 | _port.putc(0x40);//IRIS |
JamieB | 63:11ee40ec32bd | 31 | _port.putc(0); |
JamieB | 63:11ee40ec32bd | 32 | _port.putc(0); |
JamieB | 63:11ee40ec32bd | 33 | _port.putc(0); |
JamieB | 63:11ee40ec32bd | 34 | _port.putc(0x40);//ZOOM |
JamieB | 63:11ee40ec32bd | 35 | _port.putc(0); |
JamieB | 63:11ee40ec32bd | 36 | _port.putc(0); |
JamieB | 63:11ee40ec32bd | 37 | _port.putc(0); |
AndyA | 62:dcb92159ac8e | 38 | // value of 0 for the first byte indicates client device with invalid data. |
AndyA | 62:dcb92159ac8e | 39 | // So shouldn't move the motor, only read the values. |
AndyA | 62:dcb92159ac8e | 40 | // may possibly need to change this to 0x40 0x00 0x00 0x00 for each axis |
AndyA | 62:dcb92159ac8e | 41 | // (master but invalid data) |
JamieB | 63:11ee40ec32bd | 42 | // for (int i=0;i<packetSizeToSend;i++) |
JamieB | 63:11ee40ec32bd | 43 | // _port.putc(0); |
AndyA | 62:dcb92159ac8e | 44 | |
AndyA | 62:dcb92159ac8e | 45 | // checksum - if all data is 0 then cs is 0. If data above changes then change this. |
AndyA | 62:dcb92159ac8e | 46 | _port.putc(0); |
AndyA | 62:dcb92159ac8e | 47 | |
AndyA | 62:dcb92159ac8e | 48 | _port.putc(FooterByte); |
JamieB | 63:11ee40ec32bd | 49 | pc.printf("S"); |
AndyA | 62:dcb92159ac8e | 50 | } |
AndyA | 62:dcb92159ac8e | 51 | |
AndyA | 62:dcb92159ac8e | 52 | |
AndyA | 62:dcb92159ac8e | 53 | void FIZ_ArriCmotion::OnRx(void) |
AndyA | 62:dcb92159ac8e | 54 | { |
AndyA | 62:dcb92159ac8e | 55 | uint8_t dIn = _port.getc(); |
AndyA | 62:dcb92159ac8e | 56 | inputBuffer[inputPtr] = dIn; |
JamieB | 63:11ee40ec32bd | 57 | pc.printf("R"); |
AndyA | 62:dcb92159ac8e | 58 | if (inputPtr==0) { |
AndyA | 62:dcb92159ac8e | 59 | if (dIn == HeaderByte) { |
AndyA | 62:dcb92159ac8e | 60 | inputPtr++; |
AndyA | 62:dcb92159ac8e | 61 | } |
JamieB | 63:11ee40ec32bd | 62 | } else if (inputPtr == 2) { |
AndyA | 62:dcb92159ac8e | 63 | if ((dIn == 'o')||(dIn == 'O')) { // ID 'o' = Motor status. or 'O' = motor command |
AndyA | 62:dcb92159ac8e | 64 | inputPtr++; |
AndyA | 62:dcb92159ac8e | 65 | } else { // wrong ID |
AndyA | 62:dcb92159ac8e | 66 | if (dIn == HeaderByte) { |
AndyA | 62:dcb92159ac8e | 67 | inputBuffer[0] = HeaderByte; |
AndyA | 62:dcb92159ac8e | 68 | inputPtr = 1; |
AndyA | 62:dcb92159ac8e | 69 | } else { |
AndyA | 62:dcb92159ac8e | 70 | inputPtr = 0; |
AndyA | 62:dcb92159ac8e | 71 | } |
AndyA | 62:dcb92159ac8e | 72 | } |
AndyA | 62:dcb92159ac8e | 73 | } else { // waiting for data. |
AndyA | 62:dcb92159ac8e | 74 | if (inputPtr == 2) { // just got length |
JamieB | 63:11ee40ec32bd | 75 | if (inputBuffer[1]>20) // length is too high |
AndyA | 62:dcb92159ac8e | 76 | inputPtr=0; |
AndyA | 62:dcb92159ac8e | 77 | else |
AndyA | 62:dcb92159ac8e | 78 | inputPtr++; |
AndyA | 62:dcb92159ac8e | 79 | } else { |
AndyA | 62:dcb92159ac8e | 80 | inputPtr++; |
JamieB | 63:11ee40ec32bd | 81 | if ((inputPtr>4) && (inputPtr == (inputBuffer[1]))) { |
AndyA | 62:dcb92159ac8e | 82 | parsePacket(); |
AndyA | 62:dcb92159ac8e | 83 | inputPtr = 0; |
AndyA | 62:dcb92159ac8e | 84 | } |
AndyA | 62:dcb92159ac8e | 85 | } |
AndyA | 62:dcb92159ac8e | 86 | } |
AndyA | 62:dcb92159ac8e | 87 | } |
AndyA | 62:dcb92159ac8e | 88 | |
AndyA | 62:dcb92159ac8e | 89 | void FIZ_ArriCmotion::parsePacket() |
AndyA | 62:dcb92159ac8e | 90 | { |
JamieB | 63:11ee40ec32bd | 91 | pc.printf("P"); |
AndyA | 62:dcb92159ac8e | 92 | if (inputBuffer[0] != HeaderByte) |
AndyA | 62:dcb92159ac8e | 93 | return; |
AndyA | 62:dcb92159ac8e | 94 | if (inputBuffer[1] != 'o') // only parse status |
AndyA | 62:dcb92159ac8e | 95 | return; |
AndyA | 62:dcb92159ac8e | 96 | int DataLen = inputBuffer[2]; |
AndyA | 62:dcb92159ac8e | 97 | if (inputBuffer[DataLen+4] != FooterByte) |
AndyA | 62:dcb92159ac8e | 98 | return; |
AndyA | 62:dcb92159ac8e | 99 | int checksum = inputBuffer[DataLen+3]; |
AndyA | 62:dcb92159ac8e | 100 | uint8_t *dataValues = inputBuffer+3; // ptr to data part of message |
AndyA | 62:dcb92159ac8e | 101 | // checksum is xor of all bytes. |
AndyA | 62:dcb92159ac8e | 102 | int cs = 0; |
AndyA | 62:dcb92159ac8e | 103 | for (int i=0; i<DataLen; i++) { |
AndyA | 62:dcb92159ac8e | 104 | cs ^= dataValues[i]; |
AndyA | 62:dcb92159ac8e | 105 | } |
AndyA | 62:dcb92159ac8e | 106 | if (cs != checksum) { |
AndyA | 62:dcb92159ac8e | 107 | // pc.printf("FIZ Checksum Fail 0x%04X\r\n",cs); |
AndyA | 62:dcb92159ac8e | 108 | return; |
AndyA | 62:dcb92159ac8e | 109 | } |
AndyA | 62:dcb92159ac8e | 110 | |
AndyA | 62:dcb92159ac8e | 111 | // format is 2 bytes status then 16 bit value, MSB first. |
AndyA | 62:dcb92159ac8e | 112 | // This is repeated for Motor (focus?), iris and zoom |
AndyA | 62:dcb92159ac8e | 113 | uint16_t focus_Position = ((uint16_t)dataValues[2])<<8 | inputBuffer[3]; |
AndyA | 62:dcb92159ac8e | 114 | |
AndyA | 62:dcb92159ac8e | 115 | uint16_t iris_Position = ((uint16_t)inputBuffer[6])<<8 | inputBuffer[7]; |
AndyA | 62:dcb92159ac8e | 116 | |
AndyA | 62:dcb92159ac8e | 117 | uint16_t zoom_Position = ((uint16_t)inputBuffer[10])<<8 | inputBuffer[11]; |
AndyA | 62:dcb92159ac8e | 118 | |
AndyA | 62:dcb92159ac8e | 119 | // MAY NEED TO SCALE THESE |
AndyA | 62:dcb92159ac8e | 120 | _focus = focus_Position; |
AndyA | 62:dcb92159ac8e | 121 | _iris = iris_Position; |
AndyA | 62:dcb92159ac8e | 122 | _zoom = zoom_Position; |
AndyA | 62:dcb92159ac8e | 123 | |
AndyA | 62:dcb92159ac8e | 124 | newData = true; |
AndyA | 62:dcb92159ac8e | 125 | } |