Guest User

Untitled

a guest
Mar 23rd, 2015
285
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  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(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. }
RAW Paste Data