SHOW:
|
|
- or go back to the newest paste.
| 1 | #include <SoftwareSerial.h> | |
| 2 | ||
| 3 | #define Rx 10 | |
| 4 | #define Tx 11 | |
| 5 | #define BAUD 115200 | |
| 6 | #define SERIALOUT 57600 | |
| 7 | SoftwareSerial UM7Data(Rx, Tx); | |
| 8 | //AltSoftSerial UM7Data; | |
| 9 | ||
| 10 | int nState = 0; | |
| 11 | #define STATE_ZERO 0 | |
| 12 | #define STATE_S 1 | |
| 13 | #define STATE_SN 2 | |
| 14 | #define STATE_SNP 3 | |
| 15 | #define STATE_PT 4 | |
| 16 | #define STATE_READ_DATA 5 | |
| 17 | #define STATE_CHK1 6 | |
| 18 | #define STATE_CHK0 7 | |
| 19 | #define STATE_DONE 8 | |
| 20 | ||
| 21 | #define EULER_PHI_THETA 0x70 | |
| 22 | #define EULER_PSI 0x71 | |
| 23 | ||
| 24 | #define PT_HAS_DATA 0b10000000 | |
| 25 | #define PT_IS_BATCH 0b01000000 | |
| 26 | #define PT_COMM_FAIL 0b00000001 | |
| 27 | #define PT_BATCH_LEN 0x08 //Batch length = 1 | |
| 28 | ||
| 29 | #define DATA_BUFF_LEN 128 | |
| 30 | ||
| 31 | byte Data[DATA_BUFF_LEN]; | |
| 32 | int um7 = 0; | |
| 33 | bool xy; | |
| 34 | byte charBuffer = 0; | |
| 35 | int nDataByteCount = 0; | |
| 36 | ||
| 37 | typedef struct {
| |
| 38 | boolean HasData; | |
| 39 | boolean IsBatch; | |
| 40 | byte BatchLength; | |
| 41 | boolean CommFail; | |
| 42 | byte Address; | |
| 43 | byte Checksum1; | |
| 44 | byte Checksum0; | |
| 45 | byte DataLength; | |
| 46 | } UM7_PacketStruct ; | |
| 47 | ||
| 48 | UM7_PacketStruct UM7_Packet; | |
| 49 | ||
| 50 | void setup(){
| |
| 51 | UM7Data.begin(BAUD); | |
| 52 | - | Serial.begin(BAUD); |
| 52 | + | Serial.begin(SERIALOUT); |
| 53 | } | |
| 54 | ||
| 55 | void loop(){
| |
| 56 | ||
| 57 | um7 = UM7Data.available(); | |
| 58 | if (um7 > 0){
| |
| 59 | charBuffer = UM7Data.read(); | |
| 60 | switch(nState){
| |
| 61 | case STATE_ZERO : // Begin. Look for 's'. | |
| 62 | Reset(); | |
| 63 | if (charBuffer == 's'){ //0x73 = 's'
| |
| 64 | nState = STATE_S; | |
| 65 | } else {
| |
| 66 | nState = STATE_ZERO; | |
| 67 | } | |
| 68 | break; | |
| 69 | case STATE_S : // Have 's'. Look for 'n'. | |
| 70 | if (charBuffer == 'n'){ //0x6E = 'n'
| |
| 71 | nState = STATE_SN; | |
| 72 | } else {
| |
| 73 | nState = STATE_ZERO; | |
| 74 | } | |
| 75 | break; | |
| 76 | case STATE_SN : // Have 'sn'. Look for 'p'. | |
| 77 | if (charBuffer == 'p'){ //0x70 = 'p'
| |
| 78 | nState = STATE_SNP; | |
| 79 | } else {
| |
| 80 | nState = STATE_ZERO; | |
| 81 | } | |
| 82 | break; | |
| 83 | case STATE_SNP : // Have 'snp'. Read PacketType and calculate DataLength. | |
| 84 | UM7_Packet.HasData = 1 && (charBuffer & PT_HAS_DATA); | |
| 85 | UM7_Packet.IsBatch = 1 && (charBuffer & PT_IS_BATCH); | |
| 86 | UM7_Packet.BatchLength = ((charBuffer >> 2) & 0b00001111); | |
| 87 | UM7_Packet.CommFail = 1 && (charBuffer & PT_COMM_FAIL); | |
| 88 | nState = STATE_PT; | |
| 89 | if (UM7_Packet.IsBatch){
| |
| 90 | UM7_Packet.DataLength = UM7_Packet.BatchLength * 4; | |
| 91 | } else {
| |
| 92 | UM7_Packet.DataLength = 4; | |
| 93 | } | |
| 94 | break; | |
| 95 | case STATE_PT : // Have PacketType. Read Address. | |
| 96 | UM7_Packet.Address = charBuffer; | |
| 97 | nDataByteCount = 0; | |
| 98 | nState = STATE_READ_DATA; | |
| 99 | break; | |
| 100 | case STATE_READ_DATA : // Read Data. (UM7_PT.BatchLength * 4) bytes. | |
| 101 | Data[nDataByteCount] = charBuffer; | |
| 102 | nDataByteCount++; | |
| 103 | if (nDataByteCount >= UM7_Packet.DataLength){
| |
| 104 | nState = STATE_CHK1; | |
| 105 | } | |
| 106 | break; | |
| 107 | case STATE_CHK1 : // Read Checksum 1 | |
| 108 | UM7_Packet.Checksum1 = charBuffer; | |
| 109 | nState = STATE_CHK0; | |
| 110 | break; | |
| 111 | case STATE_CHK0 : // Read Checksum 0 | |
| 112 | UM7_Packet.Checksum0 = charBuffer; | |
| 113 | nState = STATE_DONE; | |
| 114 | break; | |
| 115 | case STATE_DONE : // Entire packet consumed. Process packet | |
| 116 | PrintOSC(); | |
| 117 | ||
| 118 | Reset(); | |
| 119 | nState = STATE_ZERO; | |
| 120 | ||
| 121 | xy = !xy; | |
| 122 | pollAddress(xy? EULER_PHI_THETA : EULER_PSI); | |
| 123 | break; | |
| 124 | } | |
| 125 | } | |
| 126 | else{ //Serial.println("no data");
| |
| 127 | } | |
| 128 | } | |
| 129 | ||
| 130 | void PrintOSC() {
| |
| 131 | if (UM7_Packet.Address == EULER_PHI_THETA) {
| |
| 132 | Serial.println(BitShiftCombine (Data[0], Data[1]) ); // X | |
| 133 | Serial.println(BitShiftCombine (Data[2], Data[3]) ); // Z | |
| 134 | } | |
| 135 | if (UM7_Packet.Address == EULER_PSI) {
| |
| 136 | Serial.println(BitShiftCombine(Data[0], Data[1]) ); // Y | |
| 137 | } | |
| 138 | } | |
| 139 | ||
| 140 | int32_t BitShiftCombine( unsigned char x_high, unsigned char x_low) | |
| 141 | {
| |
| 142 | return (x_high << 8) | x_low; //shift x_high over to leftmost 8 bits | |
| 143 | } | |
| 144 | ||
| 145 | void PrintDebug(){
| |
| 146 | Serial.print("HasData = ");
| |
| 147 | Serial.print(UM7_Packet.HasData,BIN); | |
| 148 | Serial.print("; IsBatch = ");
| |
| 149 | Serial.print(UM7_Packet.IsBatch,BIN); | |
| 150 | Serial.print("; BatchLength = ");
| |
| 151 | Serial.print(UM7_Packet.BatchLength,DEC); | |
| 152 | Serial.print("; ComFail = ");
| |
| 153 | Serial.print(UM7_Packet.CommFail,BIN); | |
| 154 | Serial.print("; Address = 0x");
| |
| 155 | Serial.print(UM7_Packet.Address,HEX); | |
| 156 | Serial.print("; CS1 = 0x");
| |
| 157 | Serial.print(UM7_Packet.Checksum1,HEX); | |
| 158 | Serial.print("; CS0 = 0x");
| |
| 159 | Serial.print(UM7_Packet.Checksum0,HEX); | |
| 160 | Serial.print("; DataLength = ");
| |
| 161 | Serial.print(UM7_Packet.DataLength,DEC); | |
| 162 | ||
| 163 | Serial.print("; Data = ");
| |
| 164 | for (int i=0;i<nDataByteCount;i++) | |
| 165 | Serial.write(Data[i]); | |
| 166 | Serial.print("\n");
| |
| 167 | } | |
| 168 | ||
| 169 | void pollAddress(int address){ //This void function makes a request for a packet from the IMU
| |
| 170 | byte chksum0 = 0, chksum1 = 0; | |
| 171 | unsigned int chksum = 0; | |
| 172 | ||
| 173 | chksum = 's' + 'n' + 'p' + (PT_IS_BATCH | PT_BATCH_LEN) + address; | |
| 174 | chksum1 = chksum >> 8; | |
| 175 | chksum0 = chksum & 0xFF; | |
| 176 | ||
| 177 | UM7Data.write('s');
| |
| 178 | UM7Data.write('n');
| |
| 179 | UM7Data.write('p');
| |
| 180 | UM7Data.write(PT_IS_BATCH | PT_BATCH_LEN); | |
| 181 | UM7Data.write(address); | |
| 182 | UM7Data.write(chksum1); | |
| 183 | UM7Data.write(chksum0); | |
| 184 | } | |
| 185 | ||
| 186 | void Reset(){
| |
| 187 | UM7_Packet.HasData = false; | |
| 188 | UM7_Packet.IsBatch = false; | |
| 189 | UM7_Packet.BatchLength = 0; | |
| 190 | UM7_Packet.CommFail = false; | |
| 191 | UM7_Packet.Address = 0; | |
| 192 | UM7_Packet.Checksum1 = 0; | |
| 193 | UM7_Packet.Checksum0 = 0; | |
| 194 | UM7_Packet.DataLength = 0; | |
| 195 | } |