pat9125_mbed.cpp

00001 
00002 #include "pat9125_mbed.h"
00003 #define delay(ms) wait_ms(ms)
00004 
00005 //define OTS state - X
00006 #define OTS_ROT_NO_CHANGE   0x00
00007 #define OTS_ROT_UP          0x01
00008 #define OTS_ROT_DOWN        0x02
00009 
00010 //define downscale factor for rotation of shaft  
00011 #define EXPECTED_COUNT_PER_ROUND    360
00012 #define REAL_AVG_COUNT_PER_ROUND    446 //base on: sensor Reg0x0d=0x65, shaft diameter=2mm, sensor-to-shaft distance=2mm
00013 
00014 //define OTS state - Y
00015 #define OTS_BTN_NO_CHANGE   0x00
00016 #define OTS_BTN_RELEASE     0x01
00017 #define OTS_BTN_PRESS       0x02
00018 
00019 #define LOW 0
00020 #define HIGH 1
00021 #define digitalRead(pin) *pin
00022 
00023 static pat9125_mbed_state_s *gp_state ;
00024 
00025 #define PIN_BTN_L       gp_state->pBTN_L
00026 #define PIN_BTN_R       gp_state->pBTN_R
00027 #define PIN_SEN_MOTION  gp_state->pINT
00028 
00029 #define PIN_RLED        gp_state->pRLED
00030 #define PIN_GLED        gp_state->pGLED
00031 #define digitalWrite(pin,level) *pin =  level
00032 #define LED_RED_ON      digitalWrite(PIN_RLED,LOW)
00033 #define LED_RED_OFF     digitalWrite(PIN_RLED,HIGH)
00034 #define LED_GREEN_ON    digitalWrite(PIN_GLED,LOW)
00035 #define LED_GREEN_OFF   digitalWrite(PIN_GLED,HIGH)
00036 #define attachInterrupt(pin,b,c)  //pin->enable_irq()
00037 #define digitalPinToInterrupt(pin) pin
00038 #define detachInterrupt(pin)  //pin->disable_irq()
00039 //#define LCM_DisplayString_Reset gp_state->pLCM->LCM_DisplayString_Reset
00040 //#define LCM_DisplayDecimal(a,b,c,d) gp_state->pLCM->LCM_DisplayDecimal(a,b,c,d)
00041 //#define LCM_DisplayString(a,b,c) gp_state->pLCM->LCM_DisplayString(a,b,c)
00042 
00043 #define I2C_RESET gp_state->p_i2c = gp_state->p_i2c->reset(); //workaround for nRF51 mbed
00044 
00045 
00046 static void LCM_DisplayString(unsigned char line, unsigned char position, const char *ptr)
00047 {
00048     if(gp_state->pLCM == NULL) return ;
00049     gp_state->pLCM->LCM_DisplayString(line, position, ptr);
00050 }
00051 static void LCM_DisplayDecimal(unsigned char line, unsigned char position, unsigned int hex_word, unsigned char digits)
00052 {
00053     if(gp_state->pLCM == NULL) return ;
00054     gp_state->pLCM->LCM_DisplayDecimal(line, position, hex_word, digits) ;
00055 }
00056 static void LCM_DisplayString_Reset(void)
00057 {
00058     if(gp_state->pLCM == NULL) return ;
00059     gp_state->pLCM->LCM_DisplayString_Reset();
00060 }
00061 
00062 unsigned char  xy2uart_enh=0;
00063 
00064 //for OTS
00065 signed int deltaX16;
00066 signed int deltaY16;
00067 unsigned char OTS_ROT_Status;
00068 unsigned char OTS_BTN_Status;
00069 
00070 signed long x_sum=0;
00071 signed long ds_x_sum=0;
00072 signed long pre_dsCountX=0;
00073 unsigned int OTS_BTN_Press_Cnt=0;
00074 
00075 volatile unsigned char MotionPinEventTriggered=0;
00076 
00077 // Register write function
00078 void OTS_Write_Reg(unsigned char address, unsigned char value) 
00079 {
00080     int ret ;
00081     char data_write[2];
00082     
00083     data_write[0] = address;
00084     data_write[1] = value;
00085     ret = gp_state->p_i2c->write(gp_state->slave_id, data_write, 2, 0);
00086 }
00087 
00088 // Register Read function
00089 unsigned char OTS_Read_Reg(unsigned char address)
00090 {
00091     unsigned char rdata = 0;   
00092     gp_state->p_i2c->write(gp_state->slave_id, (char *)&address, 1, 0);
00093     gp_state->p_i2c->read(gp_state->slave_id, (char *)&rdata, 1, 0);
00094 
00095     return(rdata);  
00096 }
00097 
00098 // Register write & read back check function
00099 void OTS_WriteRead_Reg(unsigned char address, unsigned char wdata) 
00100 {
00101     unsigned char rdata;
00102     do
00103     {
00104       OTS_Write_Reg(address, wdata);    // Write data to specified address
00105       rdata = OTS_Read_Reg(address);    // Read back previous written data
00106     } while(rdata != wdata);            // Check if the data is correctly written
00107    
00108 }
00109 
00110 boolean OTS_Sensor_Init(void)
00111 {
00112     unsigned char sensor_pid=0;
00113     boolean read_id_ok=false;
00114     
00115     // Read sensor_pid in address 0x00 to check if the serial link is valid, PID should be 0x31
00116     sensor_pid = OTS_Read_Reg(0x00);
00117     if(sensor_pid == 0x31)
00118     {   
00119         read_id_ok = true;
00120 
00121     //PAT9125 sensor recommended settings as below:     
00122         OTS_Write_Reg(0x7F, 0x00);  // switch to bank0, not allowed to perform OTS_WriteRead_Reg
00123         OTS_Write_Reg(0x06, 0x97); // Software Reset (i.e. set bit7 to 1), then it will reset to 0 automatically
00124 
00125         I2C_RESET;
00126 
00127         delay(1);    // delay 1ms
00128         OTS_Write_Reg(0x06, 0x17); // ensure the sensor has left the reset state.
00129     
00130         OTS_WriteRead_Reg(0x09, 0x5A);  // disable write protect
00131         OTS_WriteRead_Reg(0x0D, 0x65);  // set X-axis resolution (depends on application)
00132         OTS_WriteRead_Reg(0x0E, 0xFF);  // set Y-axis resolution (depends on application)
00133         OTS_WriteRead_Reg(0x19, 0x04);  // set 12-bit X/Y data format (depends on application)
00134         //OTS_WriteRead_Reg(0x4B, 0x04); // ONLY for VDD=VDDA=1.7~1.9V: for power saving    
00135     
00136         OTS_WriteRead_Reg(0x09, 0x00);  // enable write protect 
00137     }
00138     
00139     return read_id_ok;
00140 } 
00141 
00142 // Read motion
00143 void OTS_Read_Motion(signed int *dx16, signed int *dy16) 
00144 {
00145     int shift = (sizeof(signed int) << 3) - 12 ;
00146     signed int deltaX_l=0, deltaY_l=0, deltaXY_h=0; 
00147     signed int deltaX_h=0, deltaY_h=0;    
00148     char motion = OTS_Read_Reg(0x02) ;
00149     if(motion & 0x80)   //check motion bit in bit7
00150     {        
00151         deltaX_l = OTS_Read_Reg(0x03);
00152         deltaY_l = OTS_Read_Reg(0x04);                
00153         deltaXY_h = OTS_Read_Reg(0x12);                                     
00154 
00155         deltaX_h = (deltaXY_h<<4) & 0xF00;
00156         deltaX_h = (deltaX_h << shift) >> shift ;
00157         //if(deltaX_h & 0x800)    deltaX_h |= 0xfffff000; // 12-bit data convert to 16-bit 
00158         
00159         deltaY_h = (deltaXY_h<<8) & 0xF00;
00160         //if(deltaY_h & 0x800)    deltaY_h |= 0xfffff000; // 12-bit data convert to 16-bit
00161         deltaY_h = (deltaY_h << shift) >> shift ;
00162         
00163     }
00164     *dx16 = -(deltaX_h | deltaX_l);                 //inverse the data (depends on sensor's orientation and application)
00165     *dy16 = -(deltaY_h | deltaY_l);                 //inverse the data (depends on sensor's orientation and application) 
00166 }
00167 
00168 void OTS_Reset_Variables(void)
00169 {
00170     //reset variables
00171     x_sum=0;
00172     ds_x_sum=0; 
00173     pre_dsCountX=0;
00174     
00175     OTS_BTN_Press_Cnt=0;
00176     LCM_DisplayString_Reset();
00177 }
00178 
00179 unsigned char Detect_Rotation(signed long dsCountX) 
00180 {
00181     #define EVENT_NUM_PER_ROUND     360//10
00182     #define EVENT_COUNT_TH          (EXPECTED_COUNT_PER_ROUND / EVENT_NUM_PER_ROUND)    //360/10=36 //360/360=1
00183 
00184     signed long diff_count = 0;
00185     unsigned char OutRotState = OTS_ROT_NO_CHANGE;
00186     
00187     diff_count = dsCountX - pre_dsCountX;
00188     if( diff_count >= EVENT_COUNT_TH )
00189     {
00190         pre_dsCountX = dsCountX;
00191         OutRotState = OTS_ROT_UP;
00192     }
00193     else if( diff_count <= (-EVENT_COUNT_TH) )
00194     {
00195         pre_dsCountX = dsCountX;
00196         OutRotState = OTS_ROT_DOWN;
00197     }   
00198     
00199     return OutRotState;
00200 }
00201 
00202 signed long OTS_Resolution_Downscale(signed int delta_count) 
00203 {       
00204     x_sum += delta_count;
00205     return (x_sum * EXPECTED_COUNT_PER_ROUND / REAL_AVG_COUNT_PER_ROUND);
00206 }
00207 
00208 unsigned char OTS_Detect_Rotation(signed int dx16, signed int dy16)
00209 {   
00210     ds_x_sum = OTS_Resolution_Downscale(dx16);
00211     LCM_DisplayDecimal(1,12,ds_x_sum,4);//show downscale value
00212     
00213     return Detect_Rotation(ds_x_sum);
00214 }
00215 
00216 unsigned char OTS_Detect_Pressing(signed int dx16, signed int dy16)
00217 {
00218     #define PRESS           1
00219     #define RELEASE         0
00220         
00221     #define DX_ROTATE_TH    2   
00222     #define DY_VALID_TH     1
00223     #define ACCY_PRESS_TH   5   
00224     #define DY_RELEASE_TH   (-2)        
00225     
00226     unsigned char OutBtnState = OTS_BTN_NO_CHANGE;
00227     static signed long AccY = 0;    
00228     static unsigned char State = RELEASE; //0:release, 1:press
00229     
00230     if((dx16 >= DX_ROTATE_TH)||(dx16 <= (-DX_ROTATE_TH)))
00231     {
00232         AccY = 0;   
00233     }
00234     else
00235     {                   
00236         if(State == PRESS) 
00237         {
00238             if(dy16 <= DY_RELEASE_TH)
00239             {       
00240                 State = RELEASE;    
00241                 OutBtnState = OTS_BTN_RELEASE;  
00242             }   
00243         }
00244         else 
00245         {
00246             if(dy16 < DY_VALID_TH)
00247             {
00248                 AccY = 0;
00249             }
00250             else 
00251             {
00252                 AccY += dy16;           
00253                 if(AccY >= ACCY_PRESS_TH)
00254                 {
00255                     AccY = 0;
00256                     State = PRESS;          
00257                     OutBtnState = OTS_BTN_PRESS;
00258                 }               
00259             }
00260         }       
00261     }   
00262     
00263     return OutBtnState;
00264 }
00265 
00266 //-----------------------------------------------------------------------
00267 void OTS_MotionPin_ISR(void) 
00268 {
00269     detachInterrupt(digitalPinToInterrupt(PIN_SEN_MOTION));
00270     MotionPinEventTriggered=1;  
00271 }
00272 
00273 //-----------------------------------------------------------------------
00274 
00275 /*
00276    SerialEvent occurs whenever a new data comes in the hardware serial RX.  This routine is run between each time loop() runs, so using delay inside loop can delay response.  
00277    Multiple bytes of data may be available.
00278   
00279    Host Command Supported:   
00280    Read and write sensor register
00281    Sensor read = "r aa", aa must be hex value, example "r 0f" means read register 0x0f.
00282    Sensor write = "w aa bb", aa and bb must be hex value, example "w 02 00" means write register 0x02 with value 0x00.
00283 */
00284 #include <string>
00285 #include <algorithm> 
00286 #include <cctype>
00287 #include <locale>
00288 
00289 #define String string
00290 #define VERSRION "2.40" //PAT9125 FW version
00291 
00292 
00293 void serialProcess(String input);
00294 bool IsParamValid(char *param, String str_pre, bool hex =false);
00295 void PrintHex8(uint8_t data);
00296 bool IsDigit(char *p, bool hex);
00297 typedef unsigned char byte ;
00298 String strRead;
00299 char hexChar[3];  
00300   
00301 void println()
00302 {
00303     if(gp_state->p_pc == NULL) return ;
00304     gp_state->p_pc->printf("\n");
00305 }
00306 
00307 void println(String str)
00308 {
00309     if(gp_state->p_pc == NULL) return ;
00310     const char * c = str.c_str();
00311     gp_state->p_pc->printf(c);
00312     gp_state->p_pc->printf("\n");
00313 }
00314 void print(String str)
00315 {
00316     if(gp_state->p_pc == NULL) return ;
00317     const char * c = str.c_str();
00318     gp_state->p_pc->printf(c);
00319 }
00320 void print(signed int value)
00321 {
00322     if(gp_state->p_pc == NULL) return ;
00323     gp_state->p_pc->printf("%d",value);
00324 }
00325 
00326 
00327 string trim(const string& src, const string& c)
00328 {
00329     int p2 = src.find_last_not_of(c);
00330     if (p2 == string::npos) {
00331         return string();
00332     }
00333     int p1 = src.find_first_not_of(c);
00334     if (p1 == string::npos) {
00335         p1 = 0;
00336     }
00337  
00338     return src.substr(p1, (p2-p1)+1);
00339 }
00340 
00341 void toCharArray(char *buffer, string s)
00342 {
00343     strcpy(buffer, s.c_str());
00344 }
00345 
00346 void serialEvent() 
00347 {
00348   if(gp_state->p_pc == NULL) return ;
00349   while (gp_state->p_pc->readable())
00350   {
00351     // get the new byte:
00352     char inChar = (char)gp_state->p_pc->getc() ; //Serial.read();
00353     // if the incoming character is a newline, set a flag
00354     // so the main loop can do something about it:
00355     if ((inChar == '\n') || (inChar == '\r'))
00356     {
00357       //strRead.trim();
00358       strRead = trim(strRead, " ");
00359       strRead += " ";  // for tokenizing
00360       serialProcess(strRead);
00361       strRead = "";
00362     }
00363     else if (inChar != 0xf0)
00364     {  
00365     // add it to the inputString:
00366       strRead += inChar;
00367     }
00368   }
00369 }
00370 
00371 void serialProcess(String input)
00372 {
00373   int val;
00374   int i, j;
00375   String output;  
00376   unsigned char Address,Value;
00377     
00378   // ====================================================
00379   //  Single command goes here
00380   //  NOTE: All command compare must have " " at the back
00381   // ====================================================
00382   if (input == "pxi ")
00383   {
00384     println("PixArt-PAT9125");
00385   }
00386   else if (input == "ver ")
00387   {
00388     println(VERSRION);
00389   }
00390   else if (input == "xy2uart_on ")
00391   {
00392     xy2uart_enh=1;
00393     println("Sensor XY Output Enable");
00394   } 
00395   else if (input == "xy2uart_off ")
00396   {
00397     xy2uart_enh=0;
00398     println("Sensor XY Output Disable");
00399   }   
00400   // ====================================================
00401   //  Command with parameters goes here, like r 00, w 48 00
00402   // ====================================================
00403   else if (input != " ")    // not empty string, then proceed to decode the command
00404   {
00405     char buff[50];
00406     //input.toCharArray(buff, input.length());
00407     toCharArray(buff, input);
00408     
00409     // Get the command
00410     char *tok = strtok(buff, " ");
00411     if (!tok)
00412     {
00413       println("empty command");
00414       return;
00415     }
00416     
00417     // Get parameters, up to 4 parameters, if more than that, individual command will need to extract it on their own
00418     char *param[4];
00419     for (val = 0; val < 4; val++)
00420     {
00421       param[val] = strtok(NULL, " ");
00422       if (!param[val])  // break if we reach the end
00423         break;
00424     }
00425 
00426     // switch for command
00427     String str = String(tok);
00428     if ((str == "r") || (str == "w"))
00429     {
00430       // Address is the first byte
00431       if (!IsParamValid(param[0], "Error (r/w): address"))
00432       {
00433         return;
00434       }
00435       // convert to integer
00436       Address = strtol(param[0], NULL, 16);
00437       if (Address > 0x7F)
00438       {
00439         println("Error (r/w): address > 0x7F");
00440         return;
00441       }
00442       
00443       if (str == "w")
00444       {
00445         // Value is second byte
00446         if (!IsParamValid(param[1], "Error (w): value"))
00447         {
00448           return;
00449         }
00450         // convert to integer
00451         Value = strtol(param[1], NULL, 16);
00452         if (Value > 0xFF)
00453         {
00454           println("Error (w): value > 0xFF");
00455           return;
00456         }
00457         
00458         OTS_Write_Reg(Address, Value);
00459       }
00460       
00461       print("ADDR ");
00462       PrintHex8(Address);
00463       print(" ");
00464       PrintHex8(OTS_Read_Reg(Address));
00465       println();
00466     }
00467     else
00468     {
00469       println("Bad command");
00470     }
00471   }
00472 }
00473 
00474 // Check if param is not given or not hexa, print the error as well
00475 bool IsParamValid(char *param, String str_pre, bool hex)
00476 {
00477   if (!param)
00478   {
00479     print(str_pre);
00480     println(" not given");
00481     return false;
00482   }
00483   if (!IsDigit(param, hex))
00484   {
00485     print(str_pre);
00486     if (hex)
00487       println(" not hex");
00488     else
00489       println(" not digit");
00490     return false;
00491   }
00492   return true;
00493 }
00494 
00495 // Check whether is hexa for given char
00496 bool IsDigit(char *p, bool hex)
00497 {
00498   for (int i; i < strlen(p); i++)
00499   {
00500     if (hex)
00501     {
00502       if (!isxdigit(p[i]))
00503         return false;
00504     }
00505     else if (!isdigit(p[i]))
00506         return false;
00507   }
00508   return true;
00509 }
00510 
00511 void Hex8(uint8_t data)
00512 {
00513   byte first ;
00514   first = (data >> 4) | 48;
00515   if (first > 57) hexChar[0] = first + (byte)39;
00516   else hexChar[0] = first ;
00517   
00518   first = (data & 0x0F) | 48;
00519   if (first > 57) hexChar[1] = first + (byte)39;
00520   else hexChar[1] = first;
00521 }
00522 
00523 // Print in hex with leading zero
00524 void PrintHex8(uint8_t data)
00525 {
00526   Hex8(data);
00527   
00528   print(hexChar);
00529 }
00530 
00531 
00532 //-----------------------------------------------------------------------
00533 void loop()
00534 {
00535     
00536     if(digitalRead(PIN_BTN_L) == LOW)//or reset whenever idle_timer timeout
00537     {
00538         OTS_Reset_Variables();
00539     }
00540     
00541     if(MotionPinEventTriggered==1)      
00542     {
00543         MotionPinEventTriggered=0;//clear flag after read 'Motion Status and Data'
00544         OTS_Read_Motion(&deltaX16,&deltaY16);       
00545         
00546         if(deltaX16 || deltaY16)
00547         {
00548             OTS_ROT_Status = OTS_Detect_Rotation(deltaX16,deltaY16);
00549             OTS_BTN_Status = OTS_Detect_Pressing(deltaX16,deltaY16);
00550             
00551             if(OTS_ROT_Status == OTS_ROT_UP)        
00552             {
00553                 LED_RED_ON; 
00554                 LCM_DisplayString(1,8,"Up ");
00555             }
00556             else if(OTS_ROT_Status == OTS_ROT_DOWN)
00557             {
00558                 LED_GREEN_ON;   
00559                 LCM_DisplayString(1,8,"Dn ");
00560             }       
00561             
00562             if(OTS_BTN_Status == OTS_BTN_PRESS)
00563             {
00564                 OTS_BTN_Press_Cnt++;
00565                 if(OTS_BTN_Press_Cnt > 999) 
00566                 {   
00567                     OTS_BTN_Press_Cnt=0;
00568                 }
00569                 LCM_DisplayString(2,8,"Pre");   
00570                 LCM_DisplayDecimal(2,12,OTS_BTN_Press_Cnt,4);       
00571             }
00572             else if(OTS_BTN_Status == OTS_BTN_RELEASE)
00573             {
00574                 LCM_DisplayString(2,8,"Rel");   
00575             }
00576         }
00577         
00578         if(xy2uart_enh == 1)
00579         {   
00580             //output to UART
00581             print("DX:\t");print(deltaX16);print("\t");
00582             print("DY:\t");print(deltaY16);print("\t");
00583             print("\n");
00584         }        
00585 
00586         //re-enable interrupt for MOTION pin
00587         attachInterrupt(digitalPinToInterrupt(PIN_SEN_MOTION), OTS_MotionPin_ISR, LOW);
00588         
00589     }
00590 
00591     if(digitalRead(PIN_SEN_MOTION) == HIGH) 
00592     {
00593         LED_GREEN_OFF;
00594         LED_RED_OFF;
00595     }   
00596       
00597 }
00598 pat9125_mbed::pat9125_mbed(pat9125_mbed_state_s *state)
00599 {
00600     gp_state = state ;    
00601     gp_state->sen_status = OTS_Sensor_Init(); 
00602     print("OTS_Sensor_Init\n"); 
00603     gp_state->pINT->fall(&OTS_MotionPin_ISR); //interrupt at low state
00604 }
00605 
00606 void pat9125_mbed::task()
00607 {
00608    if(digitalRead(PIN_SEN_MOTION) == LOW)
00609    {
00610         //gp_state->p_pc->printf("Motion Low\n"); 
00611         MotionPinEventTriggered = 1 ;
00612     }
00613     loop();
00614     serialEvent();
00615 }