View difference between Paste ID: 7KFm39rN and ijLuQHdF
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
}