daily pastebin goal
34%
SHARE
TWEET

Untitled

a guest Jan 20th, 2018 55 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /*
  2. TFMINI SIGNAL CONDITIONNING AND FILTERING FOE CLOSE RANGE
  3. uint16_t strLimit = 1175;
  4. uint16_t distLow = 465;
  5. uint16_t offset = 90;
  6. Are determined experimentally by moving a reflective surface (colors
  7. cardboard) closer to sensor
  8.  
  9. uint16_t scale = 10; is adjusted according to sensor scaling == mm or cm
  10. output
  11. */
  12.  
  13. #include <SoftwareSerial.h>
  14. SoftwareSerial mySerial(18, 19); // RX, TX
  15.  
  16. uint8_t Framereceived[9];
  17. uint8_t index;
  18. uint16_t distance;
  19. uint16_t offset = 10;
  20. uint16_t scale = 1;
  21. uint16_t strength;
  22. uint16_t strLimit = 1175;
  23. uint16_t distLow = 465;
  24.  
  25. uint8_t Checksum(uint8_t *data, uint8_t length)
  26.  {
  27.      uint16_t  count;
  28.      uint16_t  Sum = 0;
  29.  
  30.      for (count = 0; count < length; count++)
  31.          Sum = Sum + data[count];
  32.      return (Sum);
  33.  }
  34.  
  35. /* READ BINARY MODE
  36. Send this command for pix format(ascii x.xx cr-lf) output is 42 57 02 00 00
  37. 00 04 06
  38. Send this command for standard output is   42 57 02 00 00 00 01 06
  39.  
  40. Standard output=
  41. Byte1-2   Byte3   Byte4   Byte5     Byte6     Byte7     Byte8    Byte9
  42. 0x59 59   Dist_L  Dist_H  Strength_L  Strength_H   Reserved   Raw.Qual  
  43. CheckSum_
  44. */
  45.  
  46. void readlaser(){    
  47.   if (mySerial.available() > 0) {
  48.       uint8_t inChar = mySerial.read();
  49.        if((inChar=='Y')&& (index==0)){
  50.         Framereceived[index]=inChar;
  51.         index++;
  52.        }
  53.        else{
  54.          if( Framereceived[0]=='Y'){
  55.           if(index<8){
  56.             Framereceived[index]=inChar;
  57.             index++;
  58.           }
  59.           else{
  60.              Framereceived[index]=inChar;
  61.              if( Framereceived[1]=='Y'){
  62.               if(Checksum(Framereceived, 8)==Framereceived[8]){
  63.               distance= (uint16_t)(Framereceived[2] +
  64. (Framereceived[3]*256));
  65.               strength = (uint16_t)(Framereceived[4] + (Framereceived[5]*256));
  66.               //Serial.print(distance);
  67.               //Serial.print("   -   ");
  68.  
  69.                 if ( (strength > strLimit) && (distance <= distLow ))  {
  70.                   distance = distance - ((abs(strength - strLimit))/3) ;
  71.                     }  
  72.                   if ((strength > strLimit) && (distance > distLow)){
  73.                     distance = 140;
  74.                       }
  75.  
  76.                     distance = constrain(distance, 140, 9000);                    
  77.                     distance = ((distance - offset)/scale);
  78.                     //Serial.print(strength);
  79.                     //Serial.print("   -   ");
  80.                     //Serial.println(distance);
  81.                    }    
  82.                }          
  83.              for(uint8_t i=0;i<9;i++){
  84.              Framereceived[i]=0;
  85.              }
  86.             index=0;
  87.           }
  88.        }
  89.     }
  90.   }
  91.  
  92.  }
  93.  
  94. void setup() {
  95.  Serial.begin(115200);
  96.  mySerial.begin(115200);
  97.  index=0;
  98.  distance=0;
  99.  strength=0;
  100. }
  101.  
  102.  
  103. void loop() {
  104.  
  105.  readlaser();
  106.  //delay(100);
  107.  
  108.  
  109.  
  110.  
  111. }
RAW Paste Data
Top