API tools faq
paste
Advertisement
Guest User

C# ISKN Script (Slate API)

a guest
Jan 19th, 2016
118
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C# 31.17 KB | None | 0 0
raw download clone embed print report
  1. #region Using directives
  2. using UnityEngine;
  3. using System.Collections;
  4. using System;
  5. using System.Collections.Generic;
  6. using System.ComponentModel;
  7. using System.IO.Ports;
  8. #endregion
  9.  
  10.  
  11. namespace ISKNStruct{
  12.     public struct Description {
  13.         public UInt16 physicalWidth;
  14.         public UInt16 physicalHeight;
  15.         public UInt16 clientWidth;
  16.         public UInt16 clientHeight;
  17.         public UInt16 clientPositionX;
  18.         public UInt16 clientPositionY;
  19.         public string deviceID;
  20.         public string firmware;
  21.     }
  22.    
  23.    
  24.     public struct Pen3DDataStruct{
  25.         public bool contact;
  26.         public Vector3 pos3D;
  27.         public Vector2 rot2D;
  28.         public UInt16 writtingArea;
  29.     }
  30.  
  31.     public struct Obj3DDataStruct{
  32.         public Vector3 pos3D;
  33.         public Vector2 rot2D;
  34.     }
  35. }
  36.  
  37. namespace ISKN{
  38.  
  39.  
  40.     public enum ObjEventType {NewObject,RemovedObject, HandShake};
  41.     public enum SlateEventType {BOOT, NEW_PAGE, NEW_LAYER, SD_IN, SD_OUT, RESET};
  42.     public enum SlateDiskState {PRESENT_REDEABLE = 0x01, READ = 0x02, WRITTE = 0x04, CHANGE = 0x08,FROM_SD = 0x10, SD_NOT_COMPATIBLE = 0x20}
  43.  
  44.     public delegate void SendVector3 (Vector3? value);
  45.     public delegate void SendDotAndRotation (Vector3? pos,Vector3? euler);
  46.     public delegate void SendPen2D (Vector2? value,bool contact);
  47.     public delegate void ObjEvent (int idObj,ObjEventType eventType);
  48.     public delegate void SlateEvent (SlateEventType slateEvent);
  49.  
  50.     public delegate void SendDescription ( ISKNStruct.Description slateDescription);
  51.     public delegate void SendInt (int value);
  52.     public delegate void SendBool (bool value);
  53.     public delegate void SendNothing ();
  54.     public delegate void SendPen3DStruct ( ISKNStruct.Pen3DDataStruct dataStruct );
  55.     public delegate void SendObj3DStruct ( ISKNStruct.Obj3DDataStruct dataStruct );
  56.  
  57.     public class ISKNSerialConnection : MonoBehaviour
  58.     {
  59.         #region Var
  60.         public Vector3 tempPosMagma;
  61.  
  62.         public Vector3 posObjet3D;
  63.         public Vector2 rotObjet2D;
  64.  
  65.  
  66.         public enum Subscription {STATE = 0x01,OBJECT_EVENT = 0x02, PEN_2D = 0x04,PEN_3D = 0x08, OBJECT_3D = 0x10, ACC = 0x20, EVENT = 0x40, DIST_STATE = 0x80,NULL = 0x00}
  67.         public enum SubscriptionRight {MAG_MOTION_3D = 0x80,MAG_MES_RAW = 0x40, MAG_MES_CAL = 0x20,MAG_MES_USED = 0x10,NULL = 0x00}
  68.        
  69.         public event SendVector3 OnNewDot = new SendVector3(delegate(Vector3? value){} );
  70.         public event SendVector3 OnNewPos = new SendVector3(delegate(Vector3? value){} );
  71.         public event SendVector3 OnNewMoment = new SendVector3(delegate(Vector3? value){} );
  72.         public event SendVector3 OnNewAcc = new SendVector3(delegate(Vector3? value){} );
  73.         public event SendDotAndRotation OnNewCamera = new SendDotAndRotation(delegate(Vector3? pos, Vector3?euler){});
  74.         public event SendPen2D OnNew2DDot = new SendPen2D(delegate(Vector2? value,bool contact){} );
  75.         public event ObjEvent OnObjectEvent = new ObjEvent(delegate(int idObj,ObjEventType EventType){} );
  76.         public event SlateEvent OnNewSlateEvent = new SlateEvent(delegate (SlateEventType slateEvent) {} );
  77.  
  78.         public event SendNothing OnTapDetected = new SendNothing(delegate (){} );
  79.         public event SendBool OnSerialPortOpen = new SendBool(delegate (bool value){} );
  80.         public event SendInt OnBatteryStateReceived = new SendInt(delegate (int value) {} );
  81.         public event SendBool OnSDCardStateReceived = new SendBool(delegate (bool value) {} );
  82.        
  83.         public event SendPen3DStruct OnNewPen3D = new SendPen3DStruct(delegate(ISKNStruct.Pen3DDataStruct dataStruct){} );
  84.         public event SendObj3DStruct OnNewObj3D = new SendObj3DStruct(delegate(ISKNStruct.Obj3DDataStruct dataStruct){} );
  85.         public event SendDescription OnSlateDescriptionReceived = new SendDescription(delegate (ISKNStruct.Description slateDescription) {} );
  86.  
  87.         private SerialPort sp;
  88.         public string[] list;
  89.  
  90.         // Decoding state of FIFO
  91.         enum state {receivingBloc, sync, waiting_header_byte_0, waiting_header_byte_1, waiting_header_byte_2}
  92.         state cur_state;
  93.         int readTimeOutCount;
  94.  
  95.     // List of used buffer type :
  96.     // Buffer = [header] + [bloc 52] + [4] + [CRC]
  97.     //  static byte maskObjEvent    = 0x02;         //  0x0002
  98.     //  static byte maskPen2D       = 0x04;         //  0x0004
  99.     //  static byte maskPen3D       = 0x08;         //  0x0008
  100.     //  static byte maskObj3D       = 0x10;         //  0x0010
  101.     //  static byte maskAcc         = 0x20;         //  0x0020
  102.     //  static byte maskEvent       = 0x40;         //  0x0040
  103.     //  static uint maskMag3D       = 0x8000;       //  0x8000
  104.  
  105.         private byte[] sub3D={
  106.             0x33,
  107.             (byte) (Subscription.STATE | Subscription.ACC | Subscription.EVENT | Subscription.OBJECT_EVENT | Subscription.PEN_2D),
  108.             (byte) (SubscriptionRight.MAG_MOTION_3D) }; // block subscription, motion 3D   
  109.  
  110.         private byte[] subdes={0x34,0x02};
  111.         private byte[] header={0xB3,0xA5,0xE1};
  112.         public  byte[] sendBuffer;
  113.         private byte[] receiveBuffer;
  114.            
  115.         public Vector3 pos;
  116.         public Vector3 mom;
  117.  
  118.         Vector3 lastMom;
  119.  
  120.         public Vector3 accData;
  121.        
  122.         // RX FIFO
  123.         private Queue FIFO = new Queue();
  124.  
  125.         // RX block
  126.         private byte[] receivedBloc;
  127.         private byte idReceivedBloc;
  128.         private byte receivedSize;
  129.         private byte receivedCount;
  130.  
  131.         #region BlocksDescription
  132.         // Description of blocks
  133.         private const byte stateID = 1;
  134.         private const byte stateSize = 2;
  135.  
  136.         private const byte descriptionID = 2;
  137.         private const byte descriptionSize = 36;
  138.  
  139.         private const byte objEventID=3;
  140.         private const byte objEventSize=2;
  141.  
  142.         private const byte blockPen2DID=4;
  143.         private const byte blockPen2DSize=9;
  144.  
  145.         private const byte blockPen3DID=5;
  146.         private const byte blockPen3DSize=13;
  147.  
  148.         private const byte object3DID = 6;
  149.         private const byte object3DSize = 10;
  150.        
  151.         private const byte blocAccID=7;
  152.         private const byte blocAccSize=13;
  153.        
  154.         private const byte eventID=8;
  155.         private const byte eventSize=1;
  156.  
  157.         private const byte stateDiskID = 9;
  158.         private const byte stateDiskSize = 7;
  159.  
  160.         private const byte fileDescriptionID = 10;
  161.         private const byte fileDescriptionSize = 13;
  162.        
  163.         private const byte fileDataID = 11;
  164.         private const byte fileDataSize = 77;
  165.        
  166.         private const byte fileOPID = 12;
  167.         private const byte fileOPSize = 33;
  168.  
  169.         private const byte bloc3DID=49;
  170.         private const byte bloc3Dsize=24;
  171.         #endregion
  172.         // time
  173.         private float elapsed;
  174.         public float deltaRefresh=0.01f; //temps de rafraichissement en secondes
  175.  
  176.         private const int toReadWithoutPen = 200;
  177.         private const int toReadWithPen = 1000;
  178.  
  179.         private int toRead = toReadWithoutPen;
  180.  
  181.         public float coefAcc = -10;
  182.         public float accFactorInterupt = 0.3f;
  183.         public float accTimeEngage = 0.5f;
  184.  
  185.         private Vector3? lastAccCapture = null;
  186.         private float accTimeBeforeEngage = 0;
  187.  
  188.         float timeLastHandshake ;
  189.  
  190.         // Needed table for CRC
  191.         private UInt16[] crc16tab= {
  192.         0x0000,0x1021,0x2042,0x3063,0x4084,0x50a5,0x60c6,0x70e7,
  193.         0x8108,0x9129,0xa14a,0xb16b,0xc18c,0xd1ad,0xe1ce,0xf1ef,
  194.         0x1231,0x0210,0x3273,0x2252,0x52b5,0x4294,0x72f7,0x62d6,
  195.         0x9339,0x8318,0xb37b,0xa35a,0xd3bd,0xc39c,0xf3ff,0xe3de,
  196.         0x2462,0x3443,0x0420,0x1401,0x64e6,0x74c7,0x44a4,0x5485,
  197.         0xa56a,0xb54b,0x8528,0x9509,0xe5ee,0xf5cf,0xc5ac,0xd58d,
  198.         0x3653,0x2672,0x1611,0x0630,0x76d7,0x66f6,0x5695,0x46b4,
  199.         0xb75b,0xa77a,0x9719,0x8738,0xf7df,0xe7fe,0xd79d,0xc7bc,
  200.         0x48c4,0x58e5,0x6886,0x78a7,0x0840,0x1861,0x2802,0x3823,
  201.         0xc9cc,0xd9ed,0xe98e,0xf9af,0x8948,0x9969,0xa90a,0xb92b,
  202.         0x5af5,0x4ad4,0x7ab7,0x6a96,0x1a71,0x0a50,0x3a33,0x2a12,
  203.         0xdbfd,0xcbdc,0xfbbf,0xeb9e,0x9b79,0x8b58,0xbb3b,0xab1a,
  204.         0x6ca6,0x7c87,0x4ce4,0x5cc5,0x2c22,0x3c03,0x0c60,0x1c41,
  205.         0xedae,0xfd8f,0xcdec,0xddcd,0xad2a,0xbd0b,0x8d68,0x9d49,
  206.         0x7e97,0x6eb6,0x5ed5,0x4ef4,0x3e13,0x2e32,0x1e51,0x0e70,
  207.         0xff9f,0xefbe,0xdfdd,0xcffc,0xbf1b,0xaf3a,0x9f59,0x8f78,
  208.         0x9188,0x81a9,0xb1ca,0xa1eb,0xd10c,0xc12d,0xf14e,0xe16f,
  209.         0x1080,0x00a1,0x30c2,0x20e3,0x5004,0x4025,0x7046,0x6067,
  210.         0x83b9,0x9398,0xa3fb,0xb3da,0xc33d,0xd31c,0xe37f,0xf35e,
  211.         0x02b1,0x1290,0x22f3,0x32d2,0x4235,0x5214,0x6277,0x7256,
  212.         0xb5ea,0xa5cb,0x95a8,0x8589,0xf56e,0xe54f,0xd52c,0xc50d,
  213.         0x34e2,0x24c3,0x14a0,0x0481,0x7466,0x6447,0x5424,0x4405,
  214.         0xa7db,0xb7fa,0x8799,0x97b8,0xe75f,0xf77e,0xc71d,0xd73c,
  215.         0x26d3,0x36f2,0x0691,0x16b0,0x6657,0x7676,0x4615,0x5634,
  216.         0xd94c,0xc96d,0xf90e,0xe92f,0x99c8,0x89e9,0xb98a,0xa9ab,
  217.         0x5844,0x4865,0x7806,0x6827,0x18c0,0x08e1,0x3882,0x28a3,
  218.         0xcb7d,0xdb5c,0xeb3f,0xfb1e,0x8bf9,0x9bd8,0xabbb,0xbb9a,
  219.         0x4a75,0x5a54,0x6a37,0x7a16,0x0af1,0x1ad0,0x2ab3,0x3a92,
  220.         0xfd2e,0xed0f,0xdd6c,0xcd4d,0xbdaa,0xad8b,0x9de8,0x8dc9,
  221.         0x7c26,0x6c07,0x5c64,0x4c45,0x3ca2,0x2c83,0x1ce0,0x0cc1,
  222.         0xef1f,0xff3e,0xcf5d,0xdf7c,0xaf9b,0xbfba,0x8fd9,0x9ff8,
  223.         0x6e17,0x7e36,0x4e55,0x5e74,0x2e93,0x3eb2,0x0ed1,0x1ef0
  224.     };
  225.  
  226.         #endregion
  227.         // Use this for initialization
  228.         void Start () {
  229.             cur_state=state.waiting_header_byte_0;     
  230.             list=SerialPort.GetPortNames();
  231.             timeLastHandshake = Time.time +2;
  232.            
  233.             // No Ports, quit
  234.             if(list == null || list.Length ==0)
  235.             {
  236.                 return;
  237.             }
  238.        
  239.             // TODO better, this code just open the last COM port available
  240.             sp=new SerialPort("\\\\.\\" + list[list.GetLength(0)-1],921600,Parity.None,8,StopBits.One);
  241.            
  242.             // 200ms Timeout
  243.             sp.ReadTimeout=1;
  244.             sp.Handshake = Handshake.None;
  245.             sp.ReadBufferSize = 8192;
  246.             sp.WriteBufferSize = 128;
  247.             sp.RtsEnable=true;
  248.             sp.DtrEnable=true;
  249.  
  250.             // Open it!
  251.             try{
  252.                 if(sp.IsOpen)
  253.                     sp.Close();
  254.  
  255.                 sp.Open();
  256.                 OnSerialPortOpen(true);
  257.                 Debug.Log(">> Start : " + list[list.GetLength(0)-1]+" is now open");
  258.                
  259.                 // Ask Slate for notifications (subscription)
  260.                 subscription();
  261.             }
  262.             catch (Exception){
  263.                 Debug.Log("Can't connect to the Slate");
  264.             }
  265.            
  266.             receiveBuffer=new byte[10000];
  267.         }
  268.  
  269.         void reconnect()
  270.         {
  271.             cur_state=state.waiting_header_byte_0; 
  272.             list=SerialPort.GetPortNames();
  273.            
  274.             // No Ports, quit
  275.             if(list == null || list.Length ==0)
  276.             {
  277.                 return;
  278.             }  
  279.  
  280.             sp=new SerialPort("\\\\.\\" + list[list.GetLength(0)-1],921600,Parity.None,8,StopBits.One);
  281.  
  282.             // 200ms timeout
  283.             sp.ReadTimeout=1;
  284.             sp.Handshake = Handshake.None;
  285.             sp.ReadBufferSize = 8192;
  286.             sp.WriteBufferSize = 128;
  287.             sp.RtsEnable=true;
  288.             sp.DtrEnable=true;
  289.  
  290.             // Open COM port
  291.             try{
  292.                 sp.Open(); 
  293.  
  294.                 Debug.Log(">> Start : " + list[list.GetLength(0)-1]+" is now open");
  295.                
  296.                 // Ask Slate for notifications (subscription)
  297.                 subscription();
  298.                 receiveBuffer=new byte[10000];
  299.             }
  300.             catch(Exception ){
  301.                 Debug.Log("Can't reconnect to the Slate");
  302.             }
  303.         }
  304.  
  305.         void OnApplicationQuit(){
  306.             if(this.sp.IsOpen){
  307.                 subScriptionInPlay((byte)Subscription.NULL,(byte)SubscriptionRight.NULL);
  308.                 sp.Close();
  309.             }
  310.         }
  311.  
  312.  
  313.         // Update is called once per frame
  314.         void FixedUpdate () {
  315.  
  316.             try
  317.             {
  318.                 int nread= sp.Read(receiveBuffer,0,toRead);    
  319.                 for(int i=0;i<nread;i++)
  320.                 {
  321.                     FIFO.Enqueue((byte)receiveBuffer[i]);
  322.                 }
  323.             }
  324.             catch(Exception)
  325.             {
  326.                 if(Time.time - timeLastHandshake > 2){
  327.                     //reconnect();
  328.                     timeLastHandshake = Time.time;
  329.                 }
  330.             }
  331.  
  332.  
  333.            
  334.             while(FIFO.Count !=0 ) //pour chaque élément reçu
  335.             {
  336.                 byte readed=(byte)FIFO.Dequeue();
  337.                
  338.                 switch(cur_state)
  339.                 {
  340.                 case state.waiting_header_byte_0 :
  341.                     if (readed==0xB3)cur_state=state.waiting_header_byte_1;
  342.                     else cur_state=state.waiting_header_byte_0;
  343.                     break;
  344.                 case state.waiting_header_byte_1 :
  345.                     if (readed==0xA5)cur_state=state.waiting_header_byte_2;
  346.                     else cur_state=state.waiting_header_byte_0;
  347.                     break;
  348.                    
  349.                 case state.waiting_header_byte_2 :
  350.                     if (readed==0xE1)cur_state=state.sync;
  351.                     else cur_state=state.waiting_header_byte_0;
  352.                     break;
  353.                    
  354.                 case state.sync :
  355.                     idReceivedBloc=readed;
  356.  
  357.                     //print(">> Update >>state.sync: id : "+readed);
  358.                    
  359.                     switch(idReceivedBloc)
  360.                     {
  361.                     case  stateID :
  362.                         receivedBloc=new byte[stateSize+2];
  363.                         receivedSize=stateSize+2;
  364.                         receivedCount=0;
  365.                         cur_state=state.receivingBloc;
  366.                         break;
  367.  
  368.                     case descriptionID:
  369.                         receivedBloc=new byte[descriptionSize+2];
  370.                         receivedSize=descriptionSize+2;
  371.                         receivedCount=0;
  372.                         cur_state=state.receivingBloc;
  373.                         break;
  374.                        
  375.                     case objEventID:
  376.                         receivedBloc=new byte[objEventSize+2];
  377.                         receivedSize=objEventSize+2;
  378.                         receivedCount=0;
  379.                         cur_state=state.receivingBloc;
  380.                         break;
  381.  
  382.                     case blockPen2DID:
  383.                         receivedBloc=new byte[blockPen2DSize+2];
  384.                         receivedSize=blockPen2DSize+2;
  385.                         receivedCount=0;
  386.                         cur_state=state.receivingBloc;
  387.                         break;
  388.                        
  389.                     case blockPen3DID:
  390.                         receivedBloc=new byte[blockPen3DSize+2];
  391.                         receivedSize=blockPen3DSize+2;
  392.                         receivedCount=0;
  393.                         cur_state=state.receivingBloc;
  394.                         break;
  395.  
  396.                     case object3DID:
  397.                         receivedBloc=new byte[object3DSize+2];
  398.                         receivedSize=object3DSize+2;
  399.                         receivedCount=0;
  400.                         cur_state=state.receivingBloc;
  401.                         break;
  402.                        
  403.                     case blocAccID :
  404.                         receivedBloc=new byte[blocAccSize+2];
  405.                         receivedSize=blocAccSize+2;
  406.                         receivedCount=0;
  407.                         cur_state=state.receivingBloc;
  408.                         break;
  409.                        
  410.                     case  eventID :
  411.                         receivedBloc=new byte[eventSize+2];
  412.                         receivedSize=eventSize+2;
  413.                         receivedCount=0;
  414.                         cur_state=state.receivingBloc;
  415.                         break;
  416.                    
  417.                     case stateDiskID :
  418.                         receivedBloc=new byte[stateDiskSize+2];
  419.                         receivedSize=stateDiskSize+2;
  420.                         receivedCount=0;
  421.                         cur_state=state.receivingBloc;
  422.                         break;
  423.  
  424.                     case fileDescriptionID :
  425.                         receivedBloc=new byte[fileDescriptionSize+2];
  426.                         receivedSize=fileDescriptionSize+2;
  427.                         receivedCount=0;
  428.                         cur_state=state.receivingBloc;
  429.                         break;
  430.  
  431.                     case fileOPID :
  432.                         receivedBloc=new byte[fileOPSize+2];
  433.                         receivedSize=fileOPSize+2;
  434.                         receivedCount=0;
  435.                         cur_state=state.receivingBloc;
  436.                         break;
  437.  
  438.                     case  bloc3DID :
  439.                         receivedBloc=new byte[bloc3Dsize+2];
  440.                         receivedSize=bloc3Dsize+2;
  441.                         receivedCount=0;
  442.                         cur_state=state.receivingBloc;
  443.                         break;
  444.                    
  445.  
  446.                     default :
  447.                         cur_state=state.waiting_header_byte_0;
  448.                         break;
  449.                     }
  450.                    
  451.                     break;
  452.                    
  453.                 case state.receivingBloc : // Block reception
  454.                     receivedBloc[receivedCount]=readed;
  455.                     receivedCount++;
  456.                     if(receivedCount>=receivedSize) // We got a valid block (full)
  457.                     {
  458.                         cur_state=state.waiting_header_byte_0;
  459.                         parseBlocReceived();
  460.                     }
  461.                     break;
  462.                    
  463.                 default :
  464.                     break;
  465.                 }
  466.             } //end while
  467.  
  468.            
  469.            
  470.         }
  471.  
  472.        
  473.         // buf = need to be the final output buffer, we a good final size
  474.         // First 4 bytes is [header] + [blockType]
  475.         UInt16 CRC_16_ccitt(Byte[] buf)
  476.         {
  477.             UInt16 CRC = 0x0000;
  478.             Int32 counter;
  479.             for( counter = 4; counter < buf.GetLength(0)-2; counter++)
  480.             {
  481.                 CRC = (UInt16)( (CRC<<8) ^ (UInt16)crc16tab[(Byte)(((CRC>>8) ^ buf[counter] )& 0x00FF) ] );
  482.             }
  483.            
  484.             return (UInt16)CRC;
  485.         }
  486.  
  487.         UInt16 CRC_16_ccitt(Byte[] buf,UInt16 length)
  488.         {
  489.             UInt16 CRC = 0x0000;
  490.             Int32 counter;
  491.             for( counter = 0; counter < length; counter++)
  492.             {
  493.                 CRC = (UInt16)( (CRC<<8) ^ (UInt16)crc16tab[(Byte)(((CRC>>8) ^ buf[counter] )& 0x00FF) ] );
  494.             }
  495.            
  496.             return (UInt16)CRC;
  497.         }
  498.        
  499.         void parseBlocReceived()
  500.         {
  501.             ushort CRCCheck;
  502.             UInt16 offsetCRC =0;
  503.             switch(idReceivedBloc)
  504.             {
  505.                
  506.             case stateID :
  507.                 offsetCRC +=stateSize;
  508.                 CRCCheck = CRC_16_ccitt(receivedBloc,offsetCRC);
  509.                
  510.                 if(CRCCheck == (receivedBloc[offsetCRC ] | (receivedBloc[offsetCRC +1] << 8 )))    
  511.                     onStateReceived();
  512.                 break;
  513.  
  514.             case descriptionID :
  515.                 offsetCRC +=descriptionSize;
  516.                 CRCCheck = CRC_16_ccitt(receivedBloc,offsetCRC);
  517.                
  518.                 if(CRCCheck == (receivedBloc[offsetCRC ] | (receivedBloc[offsetCRC +1] << 8 )))    
  519.                     onDescriptionReceived();
  520.                 break;
  521.  
  522.             case objEventID :
  523.                 offsetCRC +=objEventSize;
  524.                 CRCCheck = CRC_16_ccitt(receivedBloc,offsetCRC);
  525.  
  526.                 if(CRCCheck == (receivedBloc[offsetCRC ] | (receivedBloc[offsetCRC +1] << 8 )))    
  527.                     onObjEventReceived();
  528.                 break;
  529.  
  530.             case blockPen2DID :
  531.                 offsetCRC +=blockPen2DSize;
  532.                 CRCCheck = CRC_16_ccitt(receivedBloc,offsetCRC);
  533.                
  534.                 if(CRCCheck == (receivedBloc[offsetCRC ] | (receivedBloc[offsetCRC +1] << 8 )))
  535.                     onPen2DReceived();
  536.                 break;
  537.                
  538.             case blockPen3DID :
  539.                 offsetCRC +=blockPen3DSize;
  540.                 CRCCheck = CRC_16_ccitt(receivedBloc,offsetCRC);
  541.                
  542.                
  543.                 if(CRCCheck == (receivedBloc[offsetCRC ] | (receivedBloc[offsetCRC +1] << 8 )))
  544.                     onPen3DReceived();
  545.                 break;
  546.  
  547.             case object3DID :
  548.                 offsetCRC +=object3DSize;
  549.                 CRCCheck = CRC_16_ccitt(receivedBloc,offsetCRC);
  550.                
  551.                
  552.                 if(CRCCheck == (receivedBloc[offsetCRC ] | (receivedBloc[offsetCRC +1] << 8 )))
  553.                     onObject3DReceived();
  554.                 break;
  555.  
  556.             case blocAccID :
  557.                 offsetCRC +=blocAccSize;
  558.                 CRCCheck = CRC_16_ccitt(receivedBloc,offsetCRC);
  559.                
  560.                
  561.                 if(CRCCheck == (receivedBloc[offsetCRC ] | (receivedBloc[offsetCRC +1] << 8 )))
  562.                     onAccEventReceived();
  563.                 break;
  564.  
  565.             case eventID:
  566.                 offsetCRC +=eventSize;
  567.                 CRCCheck = CRC_16_ccitt(receivedBloc,offsetCRC);
  568.                
  569.                 if(CRCCheck == (receivedBloc[offsetCRC ] | (receivedBloc[offsetCRC +1] << 8 )))    
  570.                     onEventReceived();
  571.                 break;
  572.  
  573.             case stateDiskID:
  574.                 offsetCRC +=stateDiskSize;
  575.                 CRCCheck = CRC_16_ccitt(receivedBloc,offsetCRC);
  576.                
  577.                 if(CRCCheck == (receivedBloc[offsetCRC ] | (receivedBloc[offsetCRC +1] << 8 )))    
  578.                     onStateDiskReceived();
  579.                 break;
  580.  
  581.             case fileDescriptionID:
  582.                 offsetCRC += fileDescriptionSize;
  583.                 CRCCheck = CRC_16_ccitt(receivedBloc,offsetCRC);
  584.                
  585.                 if(CRCCheck == (receivedBloc[offsetCRC ] | (receivedBloc[offsetCRC +1] << 8 )))    
  586.                     onFileDescriptionReceived();
  587.                 break;
  588.                
  589.             case fileDataID :
  590.                 offsetCRC += fileDataSize;
  591.                 CRCCheck = CRC_16_ccitt(receivedBloc,offsetCRC);
  592.                
  593.                 if(CRCCheck == (receivedBloc[offsetCRC ] | (receivedBloc[offsetCRC +1] << 8 )))    
  594.                     onFileDataReceived();
  595.                 break;
  596.                
  597.             case fileOPID :
  598.                 offsetCRC += fileOPSize;
  599.                 CRCCheck = CRC_16_ccitt(receivedBloc,offsetCRC);
  600.                
  601.                 if(CRCCheck == (receivedBloc[offsetCRC ] | (receivedBloc[offsetCRC +1] << 8 )))    
  602.                     onFileOpReceived();
  603.                 break;
  604.                
  605.             case bloc3DID:
  606.                 offsetCRC +=bloc3Dsize;
  607.                 CRCCheck = CRC_16_ccitt(receivedBloc,offsetCRC);
  608.                
  609.                 if(CRCCheck == (receivedBloc[offsetCRC ] | (receivedBloc[offsetCRC +1] << 8 )))
  610.                     on3DReceived();
  611.                 break;
  612.  
  613.             default :
  614.                 break;
  615.             }
  616.         }
  617.        
  618.         void subscription()
  619.         {
  620.             // Global buffer
  621.             sendBuffer=new Byte[8];
  622.            
  623.             // Add the header
  624.             header.CopyTo(sendBuffer,0);
  625.  
  626.             // 3D subscribe
  627.             sub3D.CopyTo(sendBuffer,3);
  628.            
  629.             // CRC calculation
  630.             UInt16 CRC =(UInt16) CRC_16_ccitt(sendBuffer);
  631.            
  632.             // Split CRC in 2 parts
  633.             sendBuffer.SetValue( (Byte)(CRC & 0xFF),6);
  634.             sendBuffer.SetValue( (Byte)((CRC & 0xFF00)>>8),7);
  635.            
  636.             // Send the subscription block to the slate
  637.             sp.Write(sendBuffer,0,8);
  638.         }
  639.  
  640.        
  641.        
  642.         void subscription(byte[] sub)
  643.         {
  644.             if(sp.IsOpen == false){
  645.                 Debug.LogError("Port is not open");
  646.                 return;
  647.             }
  648.  
  649.  
  650.             sub3D = sub;
  651.             // Global buffer
  652.             sendBuffer=new Byte[8];
  653.            
  654.             // Add header
  655.             header.CopyTo(sendBuffer,0);
  656.  
  657.             // Set the subscription
  658.             sub.CopyTo(sendBuffer,3);
  659.            
  660.             // CRC calculation
  661.             UInt16 CRC =(UInt16) CRC_16_ccitt(sendBuffer);
  662.            
  663.             // Split CRC in 2 parts
  664.             sendBuffer.SetValue( (Byte)(CRC & 0xFF),6);
  665.             sendBuffer.SetValue( (Byte)((CRC & 0xFF00)>>8),7);
  666.            
  667.             // Send the subscription block to the slate
  668.             sp.Write(sendBuffer,0,8);
  669.         }
  670.  
  671.         public void subScriptionInPlay(byte left, byte right){
  672.             byte[] sub = new byte[3]{0x33,left,right};
  673.             subscription(sub);
  674.         }
  675.  
  676.  
  677.         #region ASK FOR UNITARY REQUEST
  678.         public void askState (){ askBlock(0x34,0x01); }
  679.         public void askDescription(){ askBlock(0x34,0x02); }
  680.         public void askPen(){ askBlock(0x34,0x03); }
  681.         public void askPen2D(){ askBlock(0x34,0x04); }
  682.         public void askPen3D(){ askBlock(0x34,0x05); }
  683.         public void askObj3D(){ askBlock(0x34,0x06); }
  684.         public void askAcc(){ askBlock(0x34,0x07); }
  685.         public void askMom(){ askBlock(0x34,0x08); }
  686.         public void askDiskState(){ askBlock(0x34,0x09); }
  687.         public void askCompletDiskList(){ askBlock(0x34,0x0A); }
  688.         #endregion
  689.         #region DISK OPERATION
  690.         void askOpDisk(byte op,UInt64 opCode,UInt16 fileID,UInt32 firstBlock){
  691.             sendBuffer=new Byte[21];
  692.            
  693.             subdes[0] = 0x35;
  694.             subdes[1] = op;
  695.            
  696.             // Add header
  697.             header.CopyTo(sendBuffer,0);
  698.  
  699.             // Add subscription
  700.             subdes.CopyTo(sendBuffer,3);
  701.            
  702.             sendBuffer.SetValue( (Byte)(opCode &  0xFF)                     ,5);
  703.             sendBuffer.SetValue( (Byte)(((opCode & 0xFF00))         >>8 )   ,6);
  704.             sendBuffer.SetValue( (Byte)(((opCode & 0xFF0000))       >>16)   ,7);
  705.             sendBuffer.SetValue( (Byte)(((opCode & 0xFF000000))     >>24)   ,8);
  706.  
  707.             sendBuffer.SetValue( (Byte)(((opCode & 0xFF00000000))           >>32)   ,9);
  708.             sendBuffer.SetValue( (Byte)(((opCode & 0xFF0000000000))         >>40)   ,10);
  709.             sendBuffer.SetValue( (Byte)(((opCode & 0xFF000000000000))       >>48)   ,11);
  710.             sendBuffer.SetValue( (Byte)(((opCode & 0xFF00000000000000))     >>56)   ,12);
  711.            
  712.             sendBuffer.SetValue( (Byte)((fileID & 0xFF))                    ,13);
  713.             sendBuffer.SetValue( (Byte)(((fileID & 0xFF00))         >>8)        ,14);
  714.            
  715.             sendBuffer.SetValue( (Byte)(firstBlock & 0xFF)                  ,15);
  716.             sendBuffer.SetValue( (Byte)(((firstBlock & 0xFF00))     >>8)        ,16);
  717.             sendBuffer.SetValue( (Byte)(((firstBlock & 0xFF0000))   >>16)   ,17);
  718.             sendBuffer.SetValue( (Byte)(((firstBlock & 0xFF000000)) >>24)   ,18);
  719.            
  720.             // Calculation of CRC
  721.             UInt16 CRC =(UInt16) CRC_16_ccitt(sendBuffer);
  722.            
  723.             // Split the CRC in 2 parts
  724.             sendBuffer.SetValue( (Byte)(CRC & 0xFF),19);
  725.             sendBuffer.SetValue( (Byte)((CRC & 0xFF00)>>8),20);
  726.            
  727.             // Send block
  728.             sp.Write(sendBuffer,0,21);
  729.         }
  730.  
  731.         #endregion
  732.         void askBlock (byte id,byte subDes){       
  733.             sendBuffer=new Byte[7];
  734.            
  735.             subdes[0] = id;
  736.             subdes[1] = subDes;
  737.  
  738.             // Add header
  739.             header.CopyTo(sendBuffer,0);
  740.            
  741.             // Add subscription
  742.             subdes.CopyTo(sendBuffer,3);
  743.            
  744.             // Calculation of CRC
  745.             UInt16 CRC =(UInt16) CRC_16_ccitt(sendBuffer);
  746.            
  747.             // Split CRC in 2 parts
  748.             sendBuffer.SetValue( (Byte)(CRC & 0xFF),5);
  749.             sendBuffer.SetValue( (Byte)((CRC & 0xFF00)>>8),6);
  750.            
  751.             // Send block
  752.             sp.Write(sendBuffer,0,7);
  753.         }
  754.  
  755.         #region ReceivedEventFunction
  756.         void onStateReceived(){
  757.             int curStateSize;
  758.             bool sdPresence;
  759.  
  760.             curStateSize    =(int)  System.BitConverter.ToChar      (receivedBloc,0);
  761.             sdPresence      =       System.BitConverter.ToBoolean   (receivedBloc,1);
  762.  
  763.             OnBatteryStateReceived(curStateSize);
  764.             OnSDCardStateReceived(sdPresence);
  765.         }
  766.  
  767.         void onDescriptionReceived(){
  768.             ISKNStruct.Description slateDescription;
  769.  
  770.             slateDescription.physicalWidth      = System.BitConverter.ToUInt16(receivedBloc,0);
  771.             slateDescription.physicalHeight     = System.BitConverter.ToUInt16(receivedBloc,2);
  772.             slateDescription.clientWidth        = System.BitConverter.ToUInt16(receivedBloc,4);
  773.             slateDescription.clientHeight       = System.BitConverter.ToUInt16(receivedBloc,6);
  774.             slateDescription.clientPositionX    = System.BitConverter.ToUInt16(receivedBloc,8);
  775.             slateDescription.clientPositionY    = System.BitConverter.ToUInt16(receivedBloc,10);
  776.             slateDescription.deviceID           = System.BitConverter.ToString(receivedBloc,12,20);
  777.             slateDescription.firmware           = System.BitConverter.ToString(receivedBloc,32);
  778.  
  779.  
  780.             Debug.Log(slateDescription.firmware );
  781.             OnSlateDescriptionReceived(slateDescription);
  782.         }
  783.  
  784.         void onObjEventReceived()
  785.         {
  786.             byte IDobject=receivedBloc[0];
  787.            
  788.             switch(receivedBloc[1])
  789.             {
  790.                 case 1 :
  791.                     // NEW OBJECT
  792.                     OnObjectEvent((int)IDobject,ObjEventType.NewObject);
  793.                     toRead = toReadWithPen;
  794.                     break;
  795.                 case 2 :
  796.                     toRead = toReadWithoutPen;
  797.                     // REMOVED OBJECT
  798.                     OnObjectEvent((int)IDobject ,ObjEventType.RemovedObject);
  799.                     break;
  800.                 case 3 :
  801.                     // HANDSHAKE
  802.                     timeLastHandshake = Time.time;
  803.                     OnObjectEvent((int)IDobject,ObjEventType.HandShake);
  804.                     break;
  805.             }
  806.         }
  807.  
  808.         void onPen2DReceived(){
  809.             bool contact;
  810.             Vector2 pos2D;
  811.             Vector2 rot2D;
  812.  
  813.  
  814.             pos2D.x = ((float)  System.BitConverter.ToUInt16(receivedBloc,0));
  815.             pos2D.y = ((float)  System.BitConverter.ToUInt16(receivedBloc,2));
  816.             rot2D.x = ((float)  System.BitConverter.ToUInt16(receivedBloc,4));
  817.             rot2D.y = ((float)  System.BitConverter.ToUInt16(receivedBloc,6));
  818.  
  819.             contact = System.BitConverter.ToBoolean(receivedBloc,8  );
  820.  
  821.             OnNew2DDot(pos2D,contact);
  822.         }
  823.  
  824.         void onPen3DReceived() {
  825.             ISKNStruct.Pen3DDataStruct dataStruct;
  826.  
  827.             dataStruct.pos3D.x          = ((float)  ushortToShor(System.BitConverter.ToUInt16(receivedBloc,0)));
  828.             dataStruct.pos3D.y          = ((float)  ushortToShor(System.BitConverter.ToUInt16(receivedBloc,2)));
  829.             dataStruct.pos3D.z          = ((float)  ushortToShor(System.BitConverter.ToUInt16(receivedBloc,4)));
  830.  
  831.             dataStruct.writtingArea     =           System.BitConverter.ToUInt16(receivedBloc,6);
  832.            
  833.             dataStruct.rot2D.x          = ((float)  ushortToShor(System.BitConverter.ToUInt16(receivedBloc,8)));
  834.             dataStruct.rot2D.y          = ((float)  ushortToShor(System.BitConverter.ToUInt16(receivedBloc,10)));
  835.  
  836.             dataStruct.contact          =           System.BitConverter.ToBoolean(receivedBloc,12);
  837.  
  838.             OnNewPen3D(dataStruct);
  839.         }
  840.        
  841.         short ushortToShor(ushort val){
  842.             short output = 0;
  843.             output = Convert.ToInt16(val.ToString("X"),16);
  844.             return output;
  845.         }
  846.  
  847.         void onObject3DReceived(){     
  848.             ISKNStruct.Obj3DDataStruct dataStruct;
  849.            
  850.             dataStruct.pos3D.x          = ((float)  System.BitConverter.ToInt16(receivedBloc,0));
  851.             dataStruct.pos3D.y          = ((float)  System.BitConverter.ToInt16(receivedBloc,2));
  852.             dataStruct.pos3D.z          = ((float)  System.BitConverter.ToInt16(receivedBloc,4));
  853.                    
  854.             dataStruct.rot2D.x          = ((float)  System.BitConverter.ToInt16(receivedBloc,6));
  855.             dataStruct.rot2D.y          = ((float)  System.BitConverter.ToInt16(receivedBloc,8));
  856.  
  857.             OnNewObj3D(dataStruct);
  858.         }
  859.        
  860.         void onAccEventReceived()
  861.         {
  862.             char op;
  863.  
  864.             accData.x= coefAcc*System.BitConverter.ToSingle(receivedBloc,0);
  865.             accData.y= coefAcc*System.BitConverter.ToSingle(receivedBloc,4);
  866.             accData.z= coefAcc*System.BitConverter.ToSingle(receivedBloc,8);
  867.            
  868.             op  =   System.BitConverter.ToChar(receivedBloc,12);
  869.  
  870.  
  871.     #if false
  872.             else if (Vector3.Distance(temp,accData) <  -0.5f * coefAcc){
  873.                 Vector3? accValueSend = accData;
  874.                 OnNewAcc (accValueSend);
  875.             }
  876.     #endif
  877.             Vector3? accValueSend = accData;
  878.  
  879.             if(lastAccCapture != null){
  880.                 accValueSend = accValueSend * 0.1f + lastAccCapture *0.9f;
  881.             }
  882.  
  883.             OnNewAcc (accValueSend);
  884.  
  885.             try{
  886.                 if( accTimeBeforeEngage  < Time.time ){
  887.                     float change = (accData.z / lastAccCapture.Value.z);
  888.  
  889.                     if((change < (1 - accFactorInterupt)) || (change > (1 + accFactorInterupt))){
  890.                         OnTapDetected ();
  891.                         accTimeBeforeEngage = Time.time + accTimeEngage;
  892.                     }
  893.                 }
  894.  
  895.             }
  896.             catch (Exception){}
  897.  
  898.             lastAccCapture = accData;
  899.         }
  900.  
  901.         void onEventReceived()
  902.         {
  903.             byte maskEvent= receivedBloc[0] ;
  904.             switch (maskEvent)
  905.             {
  906.                 case 1 : //Boot
  907.                     OnNewSlateEvent(SlateEventType.BOOT);
  908.                     break;
  909.                    
  910.                 case 2 : //"new Page"
  911.                     OnNewSlateEvent(SlateEventType.NEW_PAGE);
  912.                     break;
  913.                    
  914.                 case 3 : //"new Layer"
  915.                     OnNewSlateEvent(SlateEventType.NEW_LAYER);
  916.                     break;
  917.                    
  918.                 case 4 : //SD card in
  919.                     OnNewSlateEvent(SlateEventType.SD_IN);
  920.                     break;
  921.                    
  922.                 case 5 : //SD card out
  923.                     OnNewSlateEvent(SlateEventType.SD_OUT);
  924.                     break;
  925.                    
  926.                 case 6 : // reset
  927.                     OnNewSlateEvent(SlateEventType.RESET);
  928.                     break;
  929.                    
  930.                 default :
  931.                     break;
  932.             }
  933.  
  934.         }
  935.  
  936.         void onStateDiskReceived(){
  937. /*          char state;
  938.             UInt32 freeSpace;
  939.             UInt16 countFiles;
  940.             SlateDiskState slateDiskState = new SlateDiskState();
  941.  
  942.             state       = System.BitConverter.ToChar    (receivedBloc,0);
  943.             freeSpace   = System.BitConverter.ToUInt32  (receivedBloc,1);
  944.             countFiles  = System.BitConverter.ToUInt16  (receivedBloc,5);
  945.  
  946.             slateDiskState = (SlateDiskState) state;
  947.  
  948. */
  949.         }
  950.  
  951.         void onFileDescriptionReceived(){
  952.             UInt16  numberInListFile;
  953.             Int32   size; // Octects
  954.             UInt16  year;
  955.             char    month;
  956.             char    day;
  957.             char    hour;
  958.             char    minute;
  959.             char    second;
  960.  
  961.            
  962.             numberInListFile    = System.BitConverter.ToUInt16  (receivedBloc,0);
  963.             size                = System.BitConverter.ToInt32   (receivedBloc,2);
  964.             year                = System.BitConverter.ToUInt16  (receivedBloc,6);
  965.             month               = System.BitConverter.ToChar    (receivedBloc,8);
  966.             day                 = System.BitConverter.ToChar    (receivedBloc,9);
  967.             hour                = System.BitConverter.ToChar    (receivedBloc,10);
  968.             minute              = System.BitConverter.ToChar    (receivedBloc,11);
  969.             second              = System.BitConverter.ToChar    (receivedBloc,12);
  970.  
  971.             Debug.Log("onFileDescriptionReceived");
  972.             Debug.Log(numberInListFile);
  973.             Debug.Log(size);
  974.             Debug.Log(year);
  975.             Debug.Log(month);
  976.             Debug.Log(day);
  977.             Debug.Log(hour);
  978.             Debug.Log(minute);
  979.             Debug.Log(second);
  980.         }
  981.  
  982.         void onFileDataReceived() {
  983.             UInt16  idFile;
  984.             char    transferState;
  985.             UInt32  idBlock;
  986.             UInt32  countBlocks;
  987.             UInt16  bufferSize;
  988.             string  buffer;
  989.  
  990.             idFile              = System.BitConverter.ToUInt16  (receivedBloc,0);
  991.             transferState       = System.BitConverter.ToChar    (receivedBloc,2);
  992.             idBlock             = System.BitConverter.ToUInt32  (receivedBloc,3);
  993.             countBlocks         = System.BitConverter.ToUInt32  (receivedBloc,7);
  994.             bufferSize          = System.BitConverter.ToUInt16  (receivedBloc,11);
  995.             buffer              = System.BitConverter.ToString  (receivedBloc,13);
  996.  
  997.             Debug.Log("onFileDataReceived");
  998.             Debug.Log(idFile);
  999.             Debug.Log(transferState);
  1000.             Debug.Log(idBlock);
  1001.             Debug.Log(countBlocks);
  1002.             Debug.Log(bufferSize);
  1003.             Debug.Log(buffer);
  1004.         }
  1005.  
  1006.         void onFileOpReceived() {
  1007.             UInt64  packetId;
  1008.             UInt64  reserved;
  1009.             UInt64  typeId;
  1010.             UInt64  blockCode;
  1011.             char    state;
  1012.  
  1013.             packetId            = System.BitConverter.ToUInt64  (receivedBloc,0);
  1014.             reserved            = System.BitConverter.ToUInt64  (receivedBloc,8);
  1015.             typeId              = System.BitConverter.ToUInt64  (receivedBloc,16);
  1016.             blockCode           = System.BitConverter.ToUInt64  (receivedBloc,24);
  1017.             state               = System.BitConverter.ToChar    (receivedBloc,32);
  1018.  
  1019.             Debug.Log("onFileOpReceived");
  1020.             Debug.Log(packetId);
  1021.             Debug.Log(reserved);
  1022.             Debug.Log(typeId);
  1023.             Debug.Log(blockCode);
  1024.             Debug.Log(state);
  1025.         }
  1026.  
  1027.         void on3DReceived()
  1028.         {      
  1029.             // Position in meters
  1030.             pos.x=10.0f*System.BitConverter.ToSingle(receivedBloc,0);
  1031.             pos.y=10.0f*System.BitConverter.ToSingle(receivedBloc,4);
  1032.             pos.z=10.0f*System.BitConverter.ToSingle(receivedBloc,8);
  1033.            
  1034.            
  1035.             // Moment (not normalized)     
  1036.             mom.x=System.BitConverter.ToSingle(receivedBloc,12);
  1037.             mom.y=System.BitConverter.ToSingle(receivedBloc,16);
  1038.             mom.z=System.BitConverter.ToSingle(receivedBloc,20);
  1039.             mom.Normalize();
  1040.             mom = toUnityVector(mom,0.4f);
  1041.  
  1042.             tempPosMagma = pos;
  1043.  
  1044.             OnNewPos(pos);
  1045.             OnNewMoment(mom);
  1046.  
  1047.             Vector3 ball = pos - mom;
  1048.  
  1049.             float lenght = (mom.magnitude * pos.z) / scaleProduct(mom, Vector3.forward);
  1050.             Vector3 inter = pos  -  (mom /lenght)  /  ( 1f / mom.magnitude);
  1051.            
  1052.            
  1053.             OnNewDot(ball);
  1054.             OnNewCamera(ball,inter);
  1055.         }
  1056.         #endregion
  1057.        
  1058.         static public Vector3 toUnityVector (Vector3 tabVector,float multiple = 1)
  1059.         {
  1060.             Vector3 newPos = Vector3.zero;
  1061.            
  1062.             newPos.x = tabVector.x * multiple;
  1063.             newPos.y = tabVector.y * multiple;
  1064.             newPos.z = tabVector.z * multiple;
  1065.  
  1066.  
  1067.             return newPos;
  1068.         }  
  1069.         float scaleProduct(Vector3 a, Vector3 b){
  1070.             return (a.x * b.x) + (a.y * b.y) + (a.z * b.z);
  1071.         }
  1072.     }
  1073.  
  1074. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!