vl53lox_processing_rpi

SUBMITTED BY: Guest

DATE: May 23, 2019, 3:34 a.m.

FORMAT: Text only

SIZE: 10.2 kB

Raw Download

HITS: 433

Report
  1. import processing.io.*;
  2. I2C i2c;
  4. // MCP4725 is a Digital-to-Analog converter using I2C
  5. // datasheet: http://ww1.microchip.com/downloads/en/DeviceDoc/22039d.pdf
  7. // also see DigitalAnalog_I2C_MCP4725 for how to write the
  8. // same sketch in an object-oriented way
  9. private final static char[] hexArray = "0123456789ABCDEF".toCharArray();
  10. public static String bytesToHex(byte[] bytes) {
  11. char[] hexChars = new char[bytes.length * 2];
  12. for ( int j = 0; j < bytes.length; j++ ) {
  13. int v = bytes[j] & 0xFF;
  14. hexChars[j * 2] = hexArray[v >>> 4];
  15. hexChars[j * 2 + 1] = hexArray[v & 0x0F];
  16. }
  17. return new String(hexChars);
  18. }
  19. byte address=0x29;
  21. void writeReg(byte reg, byte val)
  22. {
  23. i2c.beginTransmission(0x29);
  24. i2c.write(reg);
  25. i2c.write(val);
  26. i2c.endTransmission();
  27. }
  28. byte [] readRegs(byte reg, int cnt)
  29. {
  30. i2c.beginTransmission(0x29);
  31. i2c.write(reg);
  32. byte[] in = i2c.read(cnt);
  33. i2c.endTransmission();
  34. return in;
  35. }
  36. byte [] readRegs(int reg, int cnt)
  37. {
  38. return readRegs((byte)reg, cnt);
  39. }
  40. byte readReg(byte reg)
  41. {
  42. return readRegs(reg,1)[0];
  43. }
  44. byte readReg(int reg)
  45. {
  46. return readReg((byte)reg);
  47. }
  48. void writeReg(int reg, int val)
  49. {
  50. writeReg((byte)reg, (byte)val);
  51. }
  52. void writeMulti(byte reg, byte [] src, int count)
  53. {
  54. i2c.beginTransmission(address);
  55. i2c.write(reg);
  57. for (int i = 0; i < count; i++)
  58. {
  59. i2c.write(src[i]);
  60. }
  62. i2c.endTransmission();
  63. }
  65. void writeReg16Bit(byte reg, short value)
  66. {
  67. i2c.beginTransmission(address);
  68. i2c.write(reg);
  69. i2c.write((value >> 8) & 0xFF); // value high byte
  70. i2c.write( value & 0xFF); // value low byte
  71. i2c.endTransmission();
  72. }
  73. void writeReg32Bit(byte reg, int value)
  74. {
  75. i2c.beginTransmission(address);
  76. i2c.write(reg);
  77. i2c.write((value >> 24) & 0xFF); // value highest byte
  78. i2c.write((value >> 16) & 0xFF);
  79. i2c.write((value >> 8) & 0xFF);
  80. i2c.write( value & 0xFF); // value lowest byte
  81. i2c.endTransmission();
  82. }
  83. byte stop_variable;
  84. byte MSRC_CONFIG_CONTROL = (byte)0x60;
  85. int measurement_timing_budget_us;
  86. int getMeasurementTimingBudget()
  87. {
  88. }
  89. boolean setMeasurementTimingBudget(int us)
  90. {
  91. }
  92. boolean setSignalRateLimit(float limit_Mcps)
  93. {
  94. if (limit_Mcps < 0 || limit_Mcps > 511.99) { return false; }
  96. // Q9.7 fixed point format (9 integer bits, 7 fractional bits)
  97. writeReg16Bit((byte)0x44, (short) (limit_Mcps * (1 << 7))); //FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT
  98. return true;
  99. }
  100. int count;
  101. boolean type_is_aperture;
  102. int timeout_start_ms;
  103. int io_timeout = 0;
  104. void startTimeout()
  105. {
  106. timeout_start_ms = millis();
  107. }
  108. boolean checkTimeoutExpired()
  109. {
  110. return (io_timeout > 0) && ((millis() - timeout_start_ms) > io_timeout);
  111. }
  112. boolean getSpadInfo()
  113. {
  114. byte tmp;
  116. writeReg(0x80, 0x01);
  117. writeReg(0xFF, 0x01);
  118. writeReg(0x00, 0x00);
  120. writeReg(0xFF, 0x06);
  121. writeReg(0x83, readReg(0x83) | 0x04);
  122. writeReg(0xFF, 0x07);
  123. writeReg(0x81, 0x01);
  125. writeReg(0x80, 0x01);
  127. writeReg(0x94, 0x6b);
  128. writeReg(0x83, 0x00);
  129. startTimeout();
  130. while (readReg(0x83) == 0x00)
  131. {
  132. if (checkTimeoutExpired()) { return false; }
  133. }
  134. writeReg(0x83, 0x01);
  135. tmp = readReg(0x92);
  137. count = tmp & (byte)0x7f;
  138. type_is_aperture = ((tmp >> (byte)7) & (byte)0x01) == (byte)0x1;
  140. writeReg(0x81, 0x00);
  141. writeReg(0xFF, 0x06);
  142. writeReg(0x83, readReg(0x83) & ~0x04);
  143. writeReg(0xFF, 0x01);
  144. writeReg(0x00, 0x01);
  146. writeReg(0xFF, 0x00);
  147. writeReg(0x80, 0x00);
  149. return true;
  150. }
  153. boolean initVL53LOX()
  154. {
  155. writeReg(0x88, 0x00);
  157. writeReg(0x80, 0x01);
  158. writeReg(0xFF, 0x01);
  159. writeReg(0x00, 0x00);
  160. stop_variable = readReg(0x91);
  161. writeReg(0x00, 0x01);
  162. writeReg(0xFF, 0x00);
  163. writeReg(0x80, 0x00);
  168. // disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks
  169. writeReg(MSRC_CONFIG_CONTROL, readReg(MSRC_CONFIG_CONTROL) | 0x12);
  171. // set final range signal rate limit to 0.25 MCPS (million counts per second)
  172. setSignalRateLimit(0.25);
  174. writeReg(0x01, 0xFF); //SYSTEM_SEQUENCE_CONFIG
  175. // VL53L0X_DataInit() end
  177. // VL53L0X_StaticInit() begin
  179. if (!getSpadInfo()) { return false; }
  180. byte spad_count = (byte)count;
  181. boolean spad_type_is_aperture = type_is_aperture;
  182. // The SPAD map (RefGoodSpadMap) is read by VL53L0X_get_info_from_device() in
  183. // the API, but the same data seems to be more easily readable from
  184. // GLOBAL_CONFIG_SPAD_ENABLES_REF_0 through _6, so read it from there
  185. byte [] ref_spad_map = readRegs((byte)0xB0, 6); //GLOBAL_CONFIG_SPAD_ENABLES_REF_0
  187. writeReg(0xFF, 0x01);
  188. writeReg(0x4F, 0x00); //DYNAMIC_SPAD_REF_EN_START_OFFSET
  189. writeReg(0x4E, 0x2C); //DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD
  190. writeReg(0xFF, 0x00);
  191. writeReg(0xB6, 0xB4); //GLOBAL_CONFIG_REF_EN_START_SELECT
  193. byte first_spad_to_enable = (byte)(spad_type_is_aperture ? 12 : 0); // 12 is the first aperture spad
  194. byte spads_enabled = 0;
  196. for (int i = 0; i < 48; i++)
  197. {
  198. if (i < first_spad_to_enable || spads_enabled == spad_count)
  199. {
  200. // This bit is lower than the first one that should be enabled, or
  201. // (reference_spad_count) bits have already been enabled, so zero this bit
  202. ref_spad_map[i / 8] &= ~(1 << (i % 8));
  203. }
  204. else if (((ref_spad_map[i / 8] >> (byte)(i % 8)) & (byte)0x1) == (byte)0x01)
  205. {
  206. spads_enabled++;
  207. }
  208. }
  209. writeMulti((byte)0xB0, ref_spad_map, 6);//GLOBAL_CONFIG_SPAD_ENABLES_REF_0
  211. // -- VL53L0X_set_reference_spads() end
  213. // -- VL53L0X_load_tuning_settings() begin
  214. // DefaultTuningSettings from vl53l0x_tuning.h
  216. writeReg(0xFF, 0x01);
  217. writeReg(0x00, 0x00);
  219. writeReg(0xFF, 0x00);
  220. writeReg(0x09, 0x00);
  221. writeReg(0x10, 0x00);
  222. writeReg(0x11, 0x00);
  224. writeReg(0x24, 0x01);
  225. writeReg(0x25, 0xFF);
  226. writeReg(0x75, 0x00);
  228. writeReg(0xFF, 0x01);
  229. writeReg(0x4E, 0x2C);
  230. writeReg(0x48, 0x00);
  231. writeReg(0x30, 0x20);
  233. writeReg(0xFF, 0x00);
  234. writeReg(0x30, 0x09);
  235. writeReg(0x54, 0x00);
  236. writeReg(0x31, 0x04);
  237. writeReg(0x32, 0x03);
  238. writeReg(0x40, 0x83);
  239. writeReg(0x46, 0x25);
  240. writeReg(0x60, 0x00);
  241. writeReg(0x27, 0x00);
  242. writeReg(0x50, 0x06);
  243. writeReg(0x51, 0x00);
  244. writeReg(0x52, 0x96);
  245. writeReg(0x56, 0x08);
  246. writeReg(0x57, 0x30);
  247. writeReg(0x61, 0x00);
  248. writeReg(0x62, 0x00);
  249. writeReg(0x64, 0x00);
  250. writeReg(0x65, 0x00);
  251. writeReg(0x66, 0xA0);
  253. writeReg(0xFF, 0x01);
  254. writeReg(0x22, 0x32);
  255. writeReg(0x47, 0x14);
  256. writeReg(0x49, 0xFF);
  257. writeReg(0x4A, 0x00);
  259. writeReg(0xFF, 0x00);
  260. writeReg(0x7A, 0x0A);
  261. writeReg(0x7B, 0x00);
  262. writeReg(0x78, 0x21);
  264. writeReg(0xFF, 0x01);
  265. writeReg(0x23, 0x34);
  266. writeReg(0x42, 0x00);
  267. writeReg(0x44, 0xFF);
  268. writeReg(0x45, 0x26);
  269. writeReg(0x46, 0x05);
  270. writeReg(0x40, 0x40);
  271. writeReg(0x0E, 0x06);
  272. writeReg(0x20, 0x1A);
  273. writeReg(0x43, 0x40);
  275. writeReg(0xFF, 0x00);
  276. writeReg(0x34, 0x03);
  277. writeReg(0x35, 0x44);
  279. writeReg(0xFF, 0x01);
  280. writeReg(0x31, 0x04);
  281. writeReg(0x4B, 0x09);
  282. writeReg(0x4C, 0x05);
  283. writeReg(0x4D, 0x04);
  285. writeReg(0xFF, 0x00);
  286. writeReg(0x44, 0x00);
  287. writeReg(0x45, 0x20);
  288. writeReg(0x47, 0x08);
  289. writeReg(0x48, 0x28);
  290. writeReg(0x67, 0x00);
  291. writeReg(0x70, 0x04);
  292. writeReg(0x71, 0x01);
  293. writeReg(0x72, 0xFE);
  294. writeReg(0x76, 0x00);
  295. writeReg(0x77, 0x00);
  297. writeReg(0xFF, 0x01);
  298. writeReg(0x0D, 0x01);
  300. writeReg(0xFF, 0x00);
  301. writeReg(0x80, 0x01);
  302. writeReg(0x01, 0xF8);
  304. writeReg(0xFF, 0x01);
  305. writeReg(0x8E, 0x01);
  306. writeReg(0x00, 0x01);
  307. writeReg(0xFF, 0x00);
  308. writeReg(0x80, 0x00);
  310. // -- VL53L0X_load_tuning_settings() end
  312. // "Set interrupt config to new sample ready"
  313. // -- VL53L0X_SetGpioConfig() begin
  315. writeReg(0x0A, 0x04);//SYSTEM_INTERRUPT_CONFIG_GPIO
  316. writeReg(0x84, readReg(0x84) & ~0x10); // active low GPIO_HV_MUX_ACTIVE_HIGH
  317. writeReg(0x0B, 0x01); //SYSTEM_INTERRUPT_CLEAR
  319. // -- VL53L0X_SetGpioConfig() end
  322. measurement_timing_budget_us = getMeasurementTimingBudget();
  323. // "Disable MSRC and TCC by default"
  324. // MSRC = Minimum Signal Rate Check
  325. // TCC = Target CentreCheck
  326. // -- VL53L0X_SetSequenceStepEnable() begin
  328. writeReg(0x01, 0xE8); //SYSTEM_SEQUENCE_CONFIG
  330. // -- VL53L0X_SetSequenceStepEnable() end
  332. // "Recalculate timing budget"
  333. setMeasurementTimingBudget(measurement_timing_budget_us);
  335. // VL53L0X_StaticInit() end
  337. // VL53L0X_PerformRefCalibration() begin (VL53L0X_perform_ref_calibration())
  339. // -- VL53L0X_perform_vhv_calibration() begin
  341. writeReg(0x01, 0x01); //SYSTEM_SEQUENCE_CONFIG
  342. if (!performSingleRefCalibration(0x40)) { return false; }
  344. // -- VL53L0X_perform_vhv_calibration() end
  346. // -- VL53L0X_perform_phase_calibration() begin
  348. writeReg(0x01, 0x02); //SYSTEM_SEQUENCE_CONFIG
  349. if (!performSingleRefCalibration(0x00)) { return false; }
  351. // -- VL53L0X_perform_phase_calibration() end
  353. // "restore the previous Sequence Config"
  354. writeReg(0x01, 0xE8); //SYSTEM_SEQUENCE_CONFIG
  356. // VL53L0X_PerformRefCalibration() end
  357. return true;
  359. }
  361. void setup() {
  362. //printArray(I2C.list());
  363. print("Hello World");
  364. i2c = new I2C(I2C.list()[0]);
  365. printArray(I2C.list());
  367. println("scanning");
  369. try
  370. {
  371. i2c.beginTransmission(0x29);
  372. i2c.write(0x00);
  373. byte[] in = i2c.read(256);
  374. i2c.endTransmission();
  376. println("found device: ");
  377. int i =0;
  378. print(" : ");
  379. for (i=0;i<16;i++)
  380. {
  382. print(bytesToHex(new byte[] {(byte)i}));
  383. print(" ");
  384. }
  385. for (i=0;i<=255;i++)
  386. {
  387. if (i % 16 == 0)
  388. {
  389. println(": ");
  390. print(bytesToHex(new byte[] {(byte)i}));
  391. print(" : ");
  392. }
  394. print( bytesToHex(new byte[] { in[i] }));
  395. print(" ");
  397. }
  399. println("!");
  401. }
  402. catch (RuntimeException e)
  403. {
  404. print("exception ");
  406. println(e);
  407. }
  408. initVL53LOX();
  414. print("Goodbye world");
  420. }
  422. void draw() {
  423. background(map(mouseX, 0, width, 0, 255));
  424. }
comments powered by Disqus