Merge pull request #2770 from pazeshun/test_wrpps_single_board

[sphand_driver] Add arduino sketch to test WrPPS Single Board

demos/sphand_ros/README.md

@@ -172,3 +172,23 @@ cat /var/log/supervisor/LaunchLogger.log
 cat /var/log/supervisor/CheckI2cdetect.log  # Log of I2C interface recognition test
 cat /var/log/supervisor/LaunchLeftGripper.log  # Log of preprocess for roslaunch (e.g., network checking)
 ```
+
+## Sensor Board Test
+
+### Palm board of gripper-v8
+
+1. Connect the palm board to Arduino (Vcc (3.3V), GND, SCL, and SDA)
+2. Install Arduino IDE
+3. Search and install VL53L0X on library manager of Arduino IDE
+4. Upload [test_palm_v8.ino](sphand_driver/arduino/test_palm_v8/test_palm_v8.ino) to Arduino
+5. Check Serial Monitor of Arduino IDE
+   - When you bring your hand close to the board, the printed values should change (`intensity` increases and `tof` decreases)
+
+### WrPPS Single Board
+
+1. Connect WrPPS Single Board to Arduino (Vcc (3.3V), GND, SCL, and SDA)
+2. Install Arduino IDE
+3. Search and install VL53L0X on library manager of Arduino IDE
+4. Upload [test_wrpps_single_board.ino](sphand_driver/arduino/test_wrpps_single_board/test_wrpps_single_board.ino) to Arduino
+5. Check Serial Monitor of Arduino IDE
+   - When you bring your hand close to the board, the printed values should change (`intensity` increases and `tof` decreases)

demos/sphand_ros/sphand_driver/arduino/test_palm_v8/test_palm_v8.ino

@@ -1,5 +1,6 @@
 #include <Wire.h>
-#include <VL53L0X.h>
+#include <VL53L0X.h>  // Search and install VL53L0X on library manager of Arduino IDE
+// https://github.com/pololu/vl53l0x-arduino
 
 /***** GLOBAL CONSTANTS *****/
 

demos/sphand_ros/sphand_driver/arduino/test_wrpps_single_board/test_wrpps_single_board.ino

@@ -0,0 +1,107 @@
+#include <Wire.h>
+#include <VL53L0X.h>  // Search and install VL53L0X on library manager of Arduino IDE
+// https://github.com/pololu/vl53l0x-arduino
+
+/***** GLOBAL CONSTANTS *****/
+
+#define VCNL4040_ADDR 0x60 //7-bit unshifted I2C address of VCNL4040
+
+//Command Registers have an upper byte and lower byte.
+#define PS_CONF1 0x03
+//#define PS_CONF2 //High byte of PS_CONF1
+#define PS_CONF3 0x04
+//#define PS_MS //High byte of PS_CONF3
+#define PS_DATA_L 0x08
+//#define PS_DATA_M //High byte of PS_DATA_L
+
+VL53L0X tof_sen;
+
+void measure_proximity()
+{
+  startProxSensor();
+  delay(10);
+  Serial.print("intensity");
+  Serial.print(": ");
+  Serial.print(readFromCommandRegister(PS_DATA_L));
+  Serial.print(", ");
+  stopProxSensor();
+  delay(1);
+}
+
+void initVCNL4040()
+{
+  delay(1);
+  //Set the options for PS_CONF3 and PS_MS bytes
+  byte conf3 = 0x00;
+  byte ms = 0b00000001; //Set IR LED current to 75mA
+  //byte ms = 0b00000010; //Set IR LED current to 100mA
+  //byte ms = 0b00000110; //Set IR LED current to 180mA
+  //byte ms = 0b00000111; //Set IR LED current to 200mA
+  writeToCommandRegister(PS_CONF3, conf3, ms);
+}
+
+void startProxSensor()
+{
+  //Clear PS_SD to turn on proximity sensing
+  //byte conf1 = 0b00000000; //Clear PS_SD bit to begin reading
+  byte conf1 = 0b00001110; //Integrate 8T, Clear PS_SD bit to begin reading
+  byte conf2 = 0b00001000; //Set PS to 16-bit
+  //byte conf2 = 0b00000000; //Clear PS to 12-bit
+  writeToCommandRegister(PS_CONF1, conf1, conf2); //Command register, low byte, high byte
+}
+
+void stopProxSensor()
+{
+  //Set PS_SD to turn off proximity sensing
+  byte conf1 = 0b00000001; //Set PS_SD bit to stop reading
+  byte conf2 = 0b00000000;
+  writeToCommandRegister(PS_CONF1, conf1, conf2); //Command register, low byte, high byte
+}
+
+//Reads a two byte value from a command register
+unsigned int readFromCommandRegister(byte commandCode)
+{
+  Wire.beginTransmission(VCNL4040_ADDR);
+  Wire.write(commandCode);
+  Wire.endTransmission(false); //Send a restart command. Do not release bus.
+
+  Wire.requestFrom(VCNL4040_ADDR, 2); //Command codes have two bytes stored in them
+
+  unsigned int data = Wire.read();
+  data |= Wire.read() << 8;
+
+  return (data);
+}
+
+//Write a two byte value to a Command Register
+void writeToCommandRegister(byte commandCode, byte lowVal, byte highVal)
+{
+  Wire.beginTransmission(VCNL4040_ADDR);
+  Wire.write(commandCode);
+  Wire.write(lowVal); //Low byte of command
+  Wire.write(highVal); //High byte of command
+  Wire.endTransmission(); //Release bus
+}
+
+void setup()
+{
+  Serial.begin(9600);
+  Wire.begin();
+
+  initVCNL4040(); //Configure sensor
+  tof_sen.init();
+  tof_sen.setTimeout(500);
+}
+
+void loop()
+{
+  measure_proximity();
+  Serial.print("tof");
+  Serial.print(": ");
+  Serial.print(tof_sen.readRangeSingleMillimeters());
+  if (tof_sen.timeoutOccurred())
+  {
+    Serial.print("TIMEOUT");
+  }
+  Serial.println();
+}