diff --git a/.travis b/.travis index 21eff912c..b223c886b 160000 --- a/.travis +++ b/.travis @@ -1 +1 @@ -Subproject commit 21eff912c50c989e48d47c4f9eb91da27775757c +Subproject commit b223c886bcda0d96c6c03b0f0ae07da768a00155 diff --git a/demos/sphand_ros/README.md b/demos/sphand_ros/README.md index 140eb4fd4..4cbd4fc98 100644 --- a/demos/sphand_ros/README.md +++ b/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) diff --git a/demos/sphand_ros/sphand_driver/arduino/test_palm_v8/test_palm_v8.ino b/demos/sphand_ros/sphand_driver/arduino/test_palm_v8/test_palm_v8.ino index b1d6bfb25..40a3c9ead 100644 --- a/demos/sphand_ros/sphand_driver/arduino/test_palm_v8/test_palm_v8.ino +++ b/demos/sphand_ros/sphand_driver/arduino/test_palm_v8/test_palm_v8.ino @@ -1,5 +1,6 @@ #include -#include +#include // Search and install VL53L0X on library manager of Arduino IDE +// https://github.com/pololu/vl53l0x-arduino /***** GLOBAL CONSTANTS *****/ diff --git a/demos/sphand_ros/sphand_driver/arduino/test_wrpps_single_board/test_wrpps_single_board.ino b/demos/sphand_ros/sphand_driver/arduino/test_wrpps_single_board/test_wrpps_single_board.ino new file mode 100644 index 000000000..25fbd22ad --- /dev/null +++ b/demos/sphand_ros/sphand_driver/arduino/test_wrpps_single_board/test_wrpps_single_board.ino @@ -0,0 +1,107 @@ +#include +#include // 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(); +}