涂 桂旺
test
Dependencies: ESP8266 HCSR04 PID
Fork of
car_test_v1
by 
涂 桂旺
Revision 3:9e51de1050a1, committed 2017-11-25
- Comitter:
- tgw
- Date:
- Sat Nov 25 03:36:58 2017 +0000
- Parent:
- 2:35738c77d454
- Commit message:
- test
Changed in this revision
diff -r 35738c77d454 -r 9e51de1050a1 APDS_9960.lib --- a/APDS_9960.lib Sat Nov 25 02:04:08 2017 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://os.mbed.com/users/kbhagat6/code/APDS_9960/#ba051af6731a
diff -r 35738c77d454 -r 9e51de1050a1 APDS_9960/glibr.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/APDS_9960/glibr.cpp Sat Nov 25 03:36:58 2017 +0000
@@ -0,0 +1,2197 @@
+#include "glibr.h"
+#include "mbed.h"
+
+
+
+
+glibr::glibr(PinName sda, PinName scl):i2c(sda, scl){
+ gesture_ud_delta_ = 0;
+ gesture_lr_delta_ = 0;
+
+ gesture_ud_count_ = 0;
+ gesture_lr_count_ = 0;
+
+ gesture_near_count_ = 0;
+ gesture_far_count_ = 0;
+
+ gesture_state_ = 0;
+ gesture_motion_ = DIR_NONE;
+}
+
+glibr::~glibr(){
+
+}
+
+ bool glibr::ginit(){
+ uint8_t id;
+
+ id=I2CreadByte(APDS9960_I2C_ADDR, APDS9960_ID);
+
+ if( (!(id == APDS9960_ID_1 || id == APDS9960_ID_2 || id == APDS9960_ID_3))||id==ERROR) {
+ return false;
+ }
+
+ if(!setMode(ALL, Off)) {
+ return false;
+ }
+ if(I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_ATIME, DEFAULT_ATIME)){
+ return false;
+ }
+ if(I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_WTIME, DEFAULT_WTIME)){
+ return false;
+ }
+ if(I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_PPULSE, DEFAULT_PROX_PPULSE)){
+ return false;
+ }
+ if(I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_POFFSET_UR, DEFAULT_POFFSET_UR)){
+ return false;
+ }
+ if(I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_POFFSET_DL, DEFAULT_POFFSET_DL)){
+ return false;
+ }
+ if(I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_CONFIG1, DEFAULT_CONFIG1)){
+ return false;
+ }
+
+ if( !setLEDDrive(DEFAULT_LDRIVE) ) {
+ return false;
+ }
+
+ if( !setProximityGain(DEFAULT_PGAIN) ) {
+ return false;
+ }
+ if( !setAmbientLightGain(DEFAULT_AGAIN) ) {
+ return false;
+ }
+ if( !setProxIntLowThresh(DEFAULT_PILT) ) {
+ return false;
+ }
+ if( !setProxIntHighThresh(DEFAULT_PIHT) ) {
+ return false;
+ }
+ if( !setLightIntLowThreshold(DEFAULT_AILT) ) {
+ return false;
+ }
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_CONFIG2, DEFAULT_CONFIG2) ) {
+ return false;
+ }
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_CONFIG3, DEFAULT_CONFIG3) ) {
+ return false;
+ }
+
+ if( !setGestureEnterThresh(DEFAULT_GPENTH) ) {
+ return false;
+ }
+ if( !setGestureExitThresh(DEFAULT_GEXTH) ) {
+ return false;
+ }
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_GCONF1, DEFAULT_GCONF1) ) {
+ return false;
+ }
+ if( !setGestureGain(DEFAULT_GGAIN) ) {
+ return false;
+ }
+ if( !setGestureLEDDrive(DEFAULT_GLDRIVE) ) {
+ return false;
+ }
+ if( !setGestureWaitTime(DEFAULT_GWTIME) ) {
+ return false;
+ }
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_GOFFSET_U, DEFAULT_GOFFSET) ) {
+ return false;
+ }
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_GOFFSET_D, DEFAULT_GOFFSET) ) {
+ return false;
+ }
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_GOFFSET_L, DEFAULT_GOFFSET) ) {
+ return false;
+ }
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_GOFFSET_R, DEFAULT_GOFFSET) ) {
+ return false;
+ }
+ if(I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_GPULSE, DEFAULT_GPULSE) ) {
+ return false;
+ }
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_GCONF3, DEFAULT_GCONF3) ) {
+ return false;
+ }
+ if( !setGestureIntEnable(DEFAULT_GIEN) ) {
+ return false;
+ }
+
+ return true;
+
+}
+
+//#if 0
+// /* Gesture config register dump */
+// uint8_t reg;
+// uint8_t val;
+//
+// for(reg = 0x80; reg <= 0xAF; reg++) {
+// if( (reg != 0x82) && \
+// (reg != 0x8A) && \
+// (reg != 0x91) && \
+// (reg != 0xA8) && \
+// (reg != 0xAC) && \
+// (reg != 0xAD) )
+// {
+// val= I2CreadByte(APDS9960_I2C_ADDR, reg);
+// if(val==ERROR){
+// printf("ERROR");
+// }
+// /*
+// print(reg, HEX);
+// print(": 0x");
+// println(val, HEX);*/
+// }
+// }
+//
+// for(reg = 0xE4; reg <= 0xE7; reg++) {
+// val= I2CreadByte(APDS9960_I2C_ADDR, reg);
+// /* Serial.print(reg, HEX);
+// Serial.print(": 0x");
+// Serial.println(val, HEX);*/
+// }
+//#endif
+
+ // return true;
+
+
+
+
+/**
+ * @brief Enables or disables a feature in the APDS-9960
+ *
+ * @param[in] mode which feature to enable
+ * @param[in] enable On (1) or Off (0)
+ * @return True if operation success. False otherwise.
+ */
+bool glibr::setMode(uint8_t mode, uint8_t enable)
+{
+ uint8_t reg_val;
+
+ /* Read current ENABLE register */
+ reg_val = getMode();
+ if( reg_val == ERROR ) {
+ return false;
+ }
+
+ /* Change bit(s) in ENABLE register */
+ enable = enable & 0x01;
+ if( mode >= 0 && mode <= 6 ) {
+ if (enable) {
+ reg_val |= (1 << mode);
+ } else {
+ reg_val &= ~(1 << mode);
+ }
+ } else if( mode == ALL ) {
+ if (enable) {
+ reg_val = 0x7F;
+ } else {
+ reg_val = 0x00;
+ }
+ }
+
+ /* Write value back to ENABLE register */
+ if(I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_ENABLE, reg_val)){
+ return false;
+ }
+
+
+
+ return true;
+}
+
+uint8_t glibr::getMode()
+{
+ uint8_t val;
+ val= I2CreadByte(APDS9960_I2C_ADDR, APDS9960_ENABLE);
+ if(val==ERROR){
+ return ERROR;
+ }
+ return val;
+}
+
+
+
+bool glibr::enableLightSensor(bool interrupts)
+{
+
+ /* Set default gain, interrupts, enable power, and enable sensor */
+ if( !setAmbientLightGain(DEFAULT_AGAIN) ) {
+ return false;
+ }
+ if( interrupts ) {
+ if( !setAmbientLightIntEnable(1) ) {
+ return false;
+ }
+ } else {
+ if( !setAmbientLightIntEnable(0) ) {
+ return false;
+ }
+ }
+ if( !enablePower() ){
+ return false;
+ }
+ if( !setMode(AMBIENT_LIGHT, 1) ) {
+ return false;
+ }
+
+ return true;
+
+}
+
+/**
+ * @brief Ends the light sensor on the APDS-9960
+ *
+ * @return True if sensor disabled correctly. False on error.
+ */
+bool glibr::disableLightSensor()
+{
+ if( !setAmbientLightIntEnable(0) ) {
+ return false;
+ }
+ if( !setMode(AMBIENT_LIGHT, 0) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Starts the proximity sensor on the APDS-9960
+ *
+ * @param[in] interrupts true to enable hardware external interrupt on proximity
+ * @return True if sensor enabled correctly. False on error.
+ */
+bool glibr::enableProximitySensor(bool interrupts)
+{
+ /* Set default gain, LED, interrupts, enable power, and enable sensor */
+ if( !setProximityGain(DEFAULT_PGAIN) ) {
+ return false;
+ }
+ if( !setLEDDrive(DEFAULT_LDRIVE) ) {
+ return false;
+ }
+ if( interrupts ) {
+ if( !setProximityIntEnable(1) ) {
+ return false;
+ }
+ } else {
+ if( !setProximityIntEnable(0) ) {
+ return false;
+ }
+ }
+ if( !enablePower() ){
+ return false;
+ }
+ if( !setMode(PROXIMITY, 1) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Ends the proximity sensor on the APDS-9960
+ *
+ * @return True if sensor disabled correctly. False on error.
+ */
+bool glibr::disableProximitySensor()
+{
+ if( !setProximityIntEnable(0) ) {
+ return false;
+ }
+ if( !setMode(PROXIMITY, 0) ) {
+ return false;
+ }
+
+ return true;
+}
+
+
+/**
+ * @brief Starts the gesture recognition engine on the APDS-9960
+ *
+ * @param[in] interrupts true to enable hardware external interrupt on gesture
+ * @return True if engine enabled correctly. False on error.
+ */
+bool glibr::enableGestureSensor(bool interrupts)
+{
+
+ /* Enable gesture mode
+ Set ENABLE to 0 (power off)
+ Set WTIME to 0xFF
+ Set AUX to LED_BOOST_300
+ Enable PON, WEN, PEN, GEN in ENABLE
+ */
+
+ resetGestureParameters();
+ if(I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_WTIME, 0xFF) ) {
+ return false;
+ }
+ if(I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_PPULSE, DEFAULT_GESTURE_PPULSE) ) {
+ return false;
+ }
+ if( !setLEDBoost(LED_BOOST_300) ) {
+ return false;
+ }
+ if( interrupts ) {
+ if( !setGestureIntEnable(1) ) {
+ return false;
+ }
+ } else {
+ if( !setGestureIntEnable(0) ) {
+ return false;
+ }
+ }
+ if( !setGestureMode(1) ) {
+ return false;
+ }
+ if( !enablePower() ){
+ return false;
+ }
+ if( !setMode(WAIT, 1) ) {
+ return false;
+ }
+ if( !setMode(PROXIMITY, 1) ) {
+ return false;
+ }
+ if( !setMode(GESTURE, 1) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Ends the gesture recognition engine on the APDS-9960
+ *
+ * @return True if engine disabled correctly. False on error.
+ */
+bool glibr::disableGestureSensor()
+{
+ resetGestureParameters();
+ if( !setGestureIntEnable(0) ) {
+ return false;
+ }
+ if( !setGestureMode(0) ) {
+ return false;
+ }
+ if( !setMode(GESTURE, 0) ) {
+ return false;
+ }
+
+ return true;
+}
+
+
+/**
+ * @brief Determines if there is a gesture available for reading
+ *
+ * @return True if gesture available. False otherwise.
+ */
+bool glibr::isGestureAvailable()
+{
+ uint8_t val;
+
+ /* Read value from GSTATUS register */
+ val=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_GSTATUS);
+ if( val==ERROR) {
+ return ERROR;
+ }
+
+ /* Shift and mask out GVALID bit */
+ val &= APDS9960_GVALID;
+
+ /* Return true/false based on GVALID bit */
+ if( val == 1) {
+ return true;
+ } else {
+ return false;
+ }
+}
+
+int glibr::readGesture()
+{
+ uint8_t fifo_level = 0;
+ // uint8_t bytes_expected= 0;
+ int check;
+ //char fifo_data[128];
+ char fifo_data[128];
+ char *fptr;
+ fptr= fifo_data;
+
+ uint8_t gstatus;
+ int motion;
+ int i;
+
+ /* Make sure that power and gesture is on and data is valid */
+ if( !isGestureAvailable() || !(getMode() & 0x41) ) {
+ return DIR_NONE;
+ }
+
+
+ /* Keep looping as long as gesture data is valid */
+ while(1) {
+
+ /* Wait some time to collect next batch of FIFO data */
+ wait(FIFO_PAUSE_TIME);
+
+ /* Get the contents of the STATUS register. Is data still valid? */
+
+ gstatus=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_GSTATUS);
+ if( gstatus==ERROR ) {
+ return ERROR;
+ }
+ /* If we have valid data, read in FIFO */
+ if( (gstatus & APDS9960_GVALID) == APDS9960_GVALID ) {
+
+ /* Read the current FIFO level */
+ fifo_level=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_GFLVL);
+ if( fifo_level==ERROR ) {
+ return ERROR;
+ }
+
+//#if DEBUG
+// Serial.print("FIFO Level: ");
+// Serial.println(fifo_level);
+//#endif
+
+ /* If there's stuff in the FIFO, read it into our data block */ //NEED TO FIGURE OUT WHAT THIS IS DOING.
+
+ if( fifo_level > 0) {
+ check = I2CReadDataBlock(APDS9960_I2C_ADDR,APDS9960_GFIFO_U,
+ fptr,
+ (fifo_level * 4) );
+
+ if( check == -1 ) {
+ return ERROR;
+ }
+
+#if DEBUG
+ //Serial.print("FIFO Dump: ");
+ for ( i = 0; i < (fifo_level * 4); i++ ) {
+ // Serial.print(fifo_data[i]);
+ // Serial.print(" ");
+ }
+ //Serial.println();
+#endif
+
+ /* If at least 1 set of data, sort the data into U/D/L/R */
+ if((fifo_level * 4) >= 4 ) {
+ for( i = 0; i < (fifo_level * 4); i += 4 ) {
+ gesture_data_.u_data[gesture_data_.sindex] = \
+ fifo_data[i + 0];
+ gesture_data_.d_data[gesture_data_.sindex] = \
+ fifo_data[i + 1];
+ gesture_data_.l_data[gesture_data_.sindex] = \
+ fifo_data[i + 2];
+ gesture_data_.r_data[gesture_data_.sindex] = \
+ fifo_data[i + 3];
+ gesture_data_.sindex++;
+ gesture_data_.total_gestures++;
+ }
+
+#if DEBUG
+ // Serial.print("Up Data: ");
+ for ( i = 0; i < gesture_data_.total_gestures; i++ ) {
+ // Serial.print(gesture_data_.u_data[i]);
+ // Serial.print(" ");
+ }
+ // Serial.println();
+#endif
+
+ /* Filter and process gesture data. Decode near/far state */
+ if( processGestureData() ) {
+ if( decodeGesture() ) {
+ //***TODO: U-Turn Gestures
+#if DEBUG
+ //Serial.println(gesture_motion_);
+#endif
+ }
+ }
+
+ /* Reset data */
+ gesture_data_.sindex = 0;
+ gesture_data_.total_gestures = 0;
+ }
+ }
+ } else {
+
+ /* Determine best guessed gesture and clean up */
+ wait(FIFO_PAUSE_TIME);
+ decodeGesture();
+ motion = gesture_motion_;
+#if DEBUG
+ // Serial.print("END: ");
+ // Serial.println(gesture_motion_);
+#endif
+ resetGestureParameters();
+ return motion;
+ }
+ }
+ // delete fptr;
+}
+/**
+ * Turn the APDS-9960 on
+ *
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::enablePower()
+{
+ if( !setMode(Power, 1) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * Turn the APDS-9960 off
+ *
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::disablePower()
+{
+ if( !setMode(Power, 0) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/*******************************************************************************
+ * Ambient light and color sensor controls
+ ******************************************************************************/
+
+/**
+ * @brief Reads the ambient (clear) light level as a 16-bit value
+ *
+ * @param[out] val value of the light sensor.
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::readAmbientLight(uint16_t &val)
+{
+ uint8_t val_byte;
+ val = 0;
+
+ /* Read value from clear channel, low byte register */
+ val_byte=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_CDATAL);
+ if( val_byte==ERROR) {
+ return false;
+ }
+ val = val_byte;
+
+ /* Read value from clear channel, high byte register */
+
+ val_byte=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_CDATAH);
+ if( val_byte==ERROR) {
+ return false;
+ }
+ val = val + ((uint16_t)val_byte << 8);
+ return true;
+}
+
+/**
+ * @brief Reads the red light level as a 16-bit value
+ *
+ * @param[out] val value of the light sensor.
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::readRedLight(uint16_t &val)
+{
+ uint8_t val_byte;
+ val = 0;
+
+ /* Read value from clear channel, low byte register */
+ val_byte=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_RDATAL);
+ if( val_byte==ERROR) {
+ return false;
+ }
+
+ val = val_byte;
+
+ /* Read value from clear channel, high byte register */
+ val_byte=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_RDATAH);
+ if( val_byte==ERROR) {
+ return false;
+ }
+ val = val + ((uint16_t)val_byte << 8);
+
+ return true;
+}
+
+/**
+ * @brief Reads the green light level as a 16-bit value
+ *
+ * @param[out] val value of the light sensor.
+ * @return True if operation successful. False otherwise.
+ */
+
+bool glibr::readGreenLight(uint16_t &val)
+{
+ uint8_t val_byte;
+ val = 0;
+
+ /* Read value from clear channel, low byte register */
+ val_byte=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_GDATAL);
+ if( val_byte==ERROR) {
+ return false;
+ }
+
+ val = val_byte;
+
+ /* Read value from clear channel, high byte register */
+ val_byte=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_GDATAH);
+ if( val_byte==ERROR) {
+ return false;
+ }
+ val = val + ((uint16_t)val_byte << 8);
+
+ return true;
+}
+
+/**
+ * @brief Reads the red light level as a 16-bit value
+ *
+ * @param[out] val value of the light sensor.
+ * @return True if operation successful. False otherwise.
+*/
+
+bool glibr::readBlueLight(uint16_t &val)
+{
+ uint8_t val_byte;
+ val = 0;
+
+ /* Read value from clear channel, low byte register */
+ val_byte=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_BDATAL);
+ if( val_byte==ERROR) {
+ return false;
+ }
+
+ val = val_byte;
+
+ /* Read value from clear channel, high byte register */
+ val_byte=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_BDATAH);
+ if( val_byte==ERROR) {
+ return false;
+ }
+ val = val + ((uint16_t)val_byte << 8);
+
+ return true;
+}
+
+/*******************************************************************************
+ * Proximity sensor controls
+ ******************************************************************************/
+
+/**
+ * @brief Reads the proximity level as an 8-bit value
+ *
+ * @param[out] val value of the proximity sensor.
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::readProximity(uint8_t &val)
+{
+ val = 0;
+
+ /* Read value from proximity data register */
+ val=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_PDATA);
+
+ if(val==ERROR){
+ return false;
+ }
+
+ return true;
+}
+
+/*******************************************************************************
+ * High-level gesture controls
+ ******************************************************************************/
+
+/**
+ * @brief Resets all the parameters in the gesture data member
+ */
+void glibr::resetGestureParameters()
+{
+ gesture_data_.sindex = 0;
+ gesture_data_.total_gestures = 0;
+
+ gesture_ud_delta_ = 0;
+ gesture_lr_delta_ = 0;
+
+ gesture_ud_count_ = 0;
+ gesture_lr_count_ = 0;
+
+ gesture_near_count_ = 0;
+ gesture_far_count_ = 0;
+
+ gesture_state_ = 0;
+ gesture_motion_ = DIR_NONE;
+}
+
+bool glibr::processGestureData()
+{
+ uint8_t u_first = 0;
+ uint8_t d_first = 0;
+ uint8_t l_first = 0;
+ uint8_t r_first = 0;
+ uint8_t u_last = 0;
+ uint8_t d_last = 0;
+ uint8_t l_last = 0;
+ uint8_t r_last = 0;
+ int ud_ratio_first;
+ int lr_ratio_first;
+ int ud_ratio_last;
+ int lr_ratio_last;
+ int ud_delta;
+ int lr_delta;
+ int i;
+
+ /* If we have less than 4 total gestures, that's not enough */
+ if( gesture_data_.total_gestures <= 4 ) {
+ return false;
+ }
+
+ /* Check to make sure our data isn't out of bounds */
+ if( (gesture_data_.total_gestures <= 32) && \
+ (gesture_data_.total_gestures > 0) ) {
+
+ /* Find the first value in U/D/L/R above the threshold */
+ for( i = 0; i < gesture_data_.total_gestures; i++ ) {
+ if( (gesture_data_.u_data[i] > GESTURE_THRESHOLD_OUT) &&
+ (gesture_data_.d_data[i] > GESTURE_THRESHOLD_OUT) &&
+ (gesture_data_.l_data[i] > GESTURE_THRESHOLD_OUT) &&
+ (gesture_data_.r_data[i] > GESTURE_THRESHOLD_OUT) ) {
+
+ u_first = gesture_data_.u_data[i];
+ d_first = gesture_data_.d_data[i];
+ l_first = gesture_data_.l_data[i];
+ r_first = gesture_data_.r_data[i];
+ break;
+ }
+ }
+
+ /* If one of the _first values is 0, then there is no good data */
+ if( (u_first == 0) || (d_first == 0) || \
+ (l_first == 0) || (r_first == 0) ) {
+
+ return false;
+ }
+ /* Find the last value in U/D/L/R above the threshold */
+ for( i = gesture_data_.total_gestures - 1; i >= 0; i-- ) {
+/* #if DEBUG
+ Serial.print(F("Finding last: "));
+ Serial.print(F("U:"));
+ Serial.print(gesture_data_.u_data[i]);
+ Serial.print(F(" D:"));
+ Serial.print(gesture_data_.d_data[i]);
+ Serial.print(F(" L:"));
+ Serial.print(gesture_data_.l_data[i]);
+ Serial.print(F(" R:"));
+ Serial.println(gesture_data_.r_data[i]);
+#endif */
+ if( (gesture_data_.u_data[i] > GESTURE_THRESHOLD_OUT) &&
+ (gesture_data_.d_data[i] > GESTURE_THRESHOLD_OUT) &&
+ (gesture_data_.l_data[i] > GESTURE_THRESHOLD_OUT) &&
+ (gesture_data_.r_data[i] > GESTURE_THRESHOLD_OUT) ) {
+
+ u_last = gesture_data_.u_data[i];
+ d_last = gesture_data_.d_data[i];
+ l_last = gesture_data_.l_data[i];
+ r_last = gesture_data_.r_data[i];
+ break;
+ }
+ }
+ }
+
+ /* Calculate the first vs. last ratio of up/down and left/right */
+ ud_ratio_first = ((u_first - d_first) * 100) / (u_first + d_first);
+ lr_ratio_first = ((l_first - r_first) * 100) / (l_first + r_first);
+ ud_ratio_last = ((u_last - d_last) * 100) / (u_last + d_last);
+ lr_ratio_last = ((l_last - r_last) * 100) / (l_last + r_last);
+
+/* #if DEBUG
+ Serial.print(F("Last Values: "));
+ Serial.print(F("U:"));
+ Serial.print(u_last);
+ Serial.print(F(" D:"));
+ Serial.print(d_last);
+ Serial.print(F(" L:"));
+ Serial.print(l_last);
+ Serial.print(F(" R:"));
+ Serial.println(r_last);
+
+ Serial.print(F("Ratios: "));
+ Serial.print(F("UD Fi: "));
+ Serial.print(ud_ratio_first);
+ Serial.print(F(" UD La: "));
+ Serial.print(ud_ratio_last);
+ Serial.print(F(" LR Fi: "));
+ Serial.print(lr_ratio_first);
+ Serial.print(F(" LR La: "));
+ Serial.println(lr_ratio_last);
+#endif */
+
+ /* Determine the difference between the first and last ratios */
+ ud_delta = ud_ratio_last - ud_ratio_first;
+ lr_delta = lr_ratio_last - lr_ratio_first;
+
+/* #if DEBUG
+ Serial.print("Deltas: ");
+ Serial.print("UD: ");
+ Serial.print(ud_delta);
+ Serial.print(" LR: ");
+ Serial.println(lr_delta);
+#endif */
+
+ /* Accumulate the UD and LR delta values */
+ gesture_ud_delta_ += ud_delta;
+ gesture_lr_delta_ += lr_delta;
+
+/* #if DEBUG
+ Serial.print("Accumulations: ");
+ Serial.print("UD: ");
+ Serial.print(gesture_ud_delta_);
+ Serial.print(" LR: ");
+ Serial.println(gesture_lr_delta_);
+#endif */
+
+ /* Determine U/D gesture */
+ if( gesture_ud_delta_ >= GESTURE_SENSITIVITY_1 ) {
+ gesture_ud_count_ = 1;
+ } else if( gesture_ud_delta_ <= -GESTURE_SENSITIVITY_1 ) {
+ gesture_ud_count_ = -1;
+ } else {
+ gesture_ud_count_ = 0;
+ }
+
+ /* Determine L/R gesture */
+ if( gesture_lr_delta_ >= GESTURE_SENSITIVITY_1 ) {
+ gesture_lr_count_ = 1;
+ } else if( gesture_lr_delta_ <= -GESTURE_SENSITIVITY_1 ) {
+ gesture_lr_count_ = -1;
+ } else {
+ gesture_lr_count_ = 0;
+ }
+
+ /* Determine Near/Far gesture */
+ if( (gesture_ud_count_ == 0) && (gesture_lr_count_ == 0) ) {
+ if( (abs(ud_delta) < GESTURE_SENSITIVITY_2) && \
+ (abs(lr_delta) < GESTURE_SENSITIVITY_2) ) {
+
+ if( (ud_delta == 0) && (lr_delta == 0) ) {
+ gesture_near_count_++;
+ } else if( (ud_delta != 0) || (lr_delta != 0) ) {
+ gesture_far_count_++;
+ }
+
+ if( (gesture_near_count_ >= 10) && (gesture_far_count_ >= 2) ) {
+ if( (ud_delta == 0) && (lr_delta == 0) ) {
+ gesture_state_ = NEAR_STATE;
+ } else if( (ud_delta != 0) && (lr_delta != 0) ) {
+ gesture_state_ = FAR_STATE;
+ }
+ return true;
+ }
+ }
+ } else {
+ if( (abs(ud_delta) < GESTURE_SENSITIVITY_2) && \
+ (abs(lr_delta) < GESTURE_SENSITIVITY_2) ) {
+
+ if( (ud_delta == 0) && (lr_delta == 0) ) {
+ gesture_near_count_++;
+ }
+
+ if( gesture_near_count_ >= 5 ) {
+ gesture_ud_count_ = 0;
+ gesture_lr_count_ = 0;
+ gesture_ud_delta_ = 0;
+ gesture_lr_delta_ = 0;
+ }
+ }
+ }
+
+// #if DEBUG
+ /* printf("UD_CT: %d\n",gesture_ud_count_);
+ printf("LR_CT: %d\n",gesture_lr_count_);
+ printf("NEAR_CT: %d\n",gesture_near_count_);
+ printf(" FAR_CT: %d\n",gesture_far_count_);
+ printf("----------"); */
+//#endif */
+
+ return false;
+}
+
+/**
+ * @brief Determines swipe direction or near/far state
+ *
+ * @return True if near/far event. False otherwise.
+ */
+bool glibr::decodeGesture()
+{
+ /* Return if near or far event is detected */
+ if( gesture_state_ == NEAR_STATE ) {
+ gesture_motion_ = DIR_NEAR;
+ return true;
+ } else if ( gesture_state_ == FAR_STATE ) {
+ gesture_motion_ = DIR_FAR;
+ return true;
+ }
+
+ /* Determine swipe direction */
+ if( (gesture_ud_count_ == -1) && (gesture_lr_count_ == 0) ) {
+ gesture_motion_ = DIR_UP;
+ } else if( (gesture_ud_count_ == 1) && (gesture_lr_count_ == 0) ) {
+ gesture_motion_ = DIR_DOWN;
+ } else if( (gesture_ud_count_ == 0) && (gesture_lr_count_ == 1) ) {
+ gesture_motion_ = DIR_RIGHT;
+ } else if( (gesture_ud_count_ == 0) && (gesture_lr_count_ == -1) ) {
+ gesture_motion_ = DIR_LEFT;
+ } else if( (gesture_ud_count_ == -1) && (gesture_lr_count_ == 1) ) {
+ if( abs(gesture_ud_delta_) > abs(gesture_lr_delta_) ) {
+ gesture_motion_ = DIR_UP;
+ } else {
+ gesture_motion_ = DIR_RIGHT;
+ }
+ } else if( (gesture_ud_count_ == 1) && (gesture_lr_count_ == -1) ) {
+ if( abs(gesture_ud_delta_) > abs(gesture_lr_delta_) ) {
+ gesture_motion_ = DIR_DOWN;
+ } else {
+ gesture_motion_ = DIR_LEFT;
+ }
+ } else if( (gesture_ud_count_ == -1) && (gesture_lr_count_ == -1) ) {
+ if( abs(gesture_ud_delta_) > abs(gesture_lr_delta_) ) {
+ gesture_motion_ = DIR_UP;
+ } else {
+ gesture_motion_ = DIR_LEFT;
+ }
+ } else if( (gesture_ud_count_ == 1) && (gesture_lr_count_ == 1) ) {
+ if( abs(gesture_ud_delta_) > abs(gesture_lr_delta_) ) {
+ gesture_motion_ = DIR_DOWN;
+ } else {
+ gesture_motion_ = DIR_RIGHT;
+ }
+ } else {
+ return false;
+ }
+
+ return true;
+}
+
+/*******************************************************************************
+ * Getters and setters for register values
+ ******************************************************************************/
+
+/**
+ * @brief Returns the lower threshold for proximity detection
+ *
+ * @return lower threshold
+ */
+
+ uint8_t glibr::getProxIntLowThresh()
+{
+ uint8_t val;
+
+ /* Read value from PILT register */
+ /* if( !wireReadDataByte(APDS9960_PILT, val) ) {
+ val = 0;
+ }*/
+ val=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_PILT);
+ if(val==ERROR){
+ val=0;
+ }
+
+ return val;
+}
+
+ /**
+ * @brief Sets the lower threshold for proximity detection
+ *
+ * @param[in] threshold the lower proximity threshold
+ * @return True if operation successful. False otherwise.
+ */
+ bool glibr::setProxIntLowThresh(uint8_t threshold)
+{
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_PILT, threshold) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Returns the high threshold for proximity detection
+ *
+ * @return high threshold
+ */
+uint8_t glibr::getProxIntHighThresh()
+{
+ uint8_t val;
+
+ /* Read value from PIHT register */
+ val=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_PILT);
+ if( val==ERROR ) {
+ val = 0;
+ }
+
+ return val;
+}
+
+/**
+ * @brief Sets the high threshold for proximity detection
+ *
+ * @param[in] threshold the high proximity threshold
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::setProxIntHighThresh(uint8_t threshold)
+{
+
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_PIHT, threshold) ) {
+ return false;
+ }
+
+ return true;
+}
+
+ /**
+ * @brief Returns LED drive strength for proximity and ALS
+ *
+ * Value LED Current
+ * 0 100 mA
+ * 1 50 mA
+ * 2 25 mA
+ * 3 12.5 mA
+ *
+ * @return the value of the LED drive strength. 0xFF on failure.
+ */
+uint8_t glibr::getLEDDrive()
+{
+ uint8_t val;
+
+ /* Read value from CONTROL register */
+ val=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_CONTROL);
+ if( val == ERROR ){//!wireReadDataByte(APDS9960_CONTROL, val) ) {
+ return ERROR;
+ }
+
+ /* Shift and mask out LED drive bits */
+ val = (val >> 6) & 0x03;//0b00000011;
+
+ return val;
+}
+
+ /**
+ * @brief Sets the LED drive strength for proximity and ALS
+ *
+ * Value LED Current
+ * 0 100 mA
+ * 1 50 mA
+ * 2 25 mA
+ * 3 12.5 mA
+ *
+ * @param[in] drive the value (0-3) for the LED drive strength
+ * @return True if operation successful. False otherwise.
+ */
+
+bool glibr::setLEDDrive(uint8_t drive)
+{
+ uint8_t val;
+
+ /* Read value from CONTROL register */
+ val=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_CONTROL);
+ if(val==ERROR){
+ return false;
+ }
+ /* Set bits in register to given value */
+ //drive &= 0b00000011
+ drive &= 0x03;
+ drive = drive << 6;
+ //val &= 0b00111111;
+ val &= 0x3F;
+ val |= drive;
+
+ /* Write register value back into CONTROL register */
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_CONTROL, val) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Returns receiver gain for proximity detection
+ *
+ * Value Gain
+ * 0 1x
+ * 1 2x
+ * 2 4x
+ * 3 8x
+ *
+ * @return the value of the proximity gain. 0xFF on failure.
+ */
+uint8_t glibr::getProximityGain()
+{
+ uint8_t val;
+
+ /* Read value from CONTROL register */
+ val=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_CONTROL);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_CONTROL, val) ) {
+ return ERROR;
+ }
+
+ /* Shift and mask out PDRIVE bits */
+ val = (val >> 2) & 0x03;//0b00000011;
+
+ return val;
+}
+
+/**
+ * @brief Sets the receiver gain for proximity detection
+ *
+ * Value Gain
+ * 0 1x
+ * 1 2x
+ * 2 4x
+ * 3 8x
+ *
+ * @param[in] drive the value (0-3) for the gain
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::setProximityGain(uint8_t drive)
+{
+ uint8_t val;
+
+ /* Read value from CONTROL register */
+
+ val=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_CONTROL);
+
+ if(val==ERROR){
+ return false;
+ }
+ /* Set bits in register to given value */
+ //drive &= 0b00000011;
+ drive &=0x03;
+ drive = drive << 2;
+ //val &= 0b11110011
+ val &= 0xF3;
+ val |= drive;
+
+ /* Write register value back into CONTROL register */
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_CONTROL, val) ) {
+ return false;
+ }
+ return true;
+}
+
+/**
+ * @brief Returns receiver gain for the ambient light sensor (ALS)
+ *
+ * Value Gain
+ * 0 1x
+ * 1 4x
+ * 2 16x
+ * 3 64x
+ *
+ * @return the value of the ALS gain. 0xFF on failure.
+ */
+uint8_t glibr::getAmbientLightGain()
+{
+ uint8_t val;
+
+ /* Read value from CONTROL register */
+ val=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_CONTROL);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_CONTROL, val) ) {
+ return ERROR;
+ }
+
+ /* Shift and mask out ADRIVE bits */
+ val &= 0x03;//0b00000011;
+
+ return val;
+}
+
+/**
+ * @brief Sets the receiver gain for the ambient light sensor (ALS)
+ *
+ * Value Gain
+ * 0 1x
+ * 1 4x
+ * 2 16x
+ * 3 64x
+ *
+ * @param[in] drive the value (0-3) for the gain
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::setAmbientLightGain(uint8_t drive){
+
+ uint8_t val;
+
+ /* Read value from CONTROL register */
+
+ val=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_CONTROL);
+
+ if(val==ERROR){
+ return false;
+ }
+ /* Set bits in register to given value */
+ //drive &= 0b00000011;
+ drive &=0x03;
+ drive = drive << 2;
+ //val &=0b11111100
+ val &= 0xF3;
+ val |= drive;
+
+ /* Write register value back into CONTROL register */
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_CONTROL, val) ) {
+ return false;
+ }
+ return true;
+}
+
+/**
+ * @brief Get the current LED boost value
+ *
+ * Value Boost Current
+ * 0 100%
+ * 1 150%
+ * 2 200%
+ * 3 300%
+ *
+ * @return The LED boost value. 0xFF on failure.
+ */
+uint8_t glibr::getLEDBoost() {
+ uint8_t val;
+
+ /* Read value from CONFIG2 register */
+ val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_CONFIG2);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_CONFIG2, val) ) {
+ return ERROR;
+ }
+
+ /* Shift and mask out LED_BOOST bits */
+ val = (val >> 4) & 0x03;//0b00000011;
+
+ return val;
+}
+
+/**
+ * @brief Sets the LED current boost value
+ *
+ * Value Boost Current
+ * 0 100%
+ * 1 150%
+ * 2 200%
+ * 3 300%
+ *
+ * @param[in] drive the value (0-3) for current boost (100-300%)
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::setLEDBoost(uint8_t boost)
+{
+ uint8_t val;
+
+ /* Read value from CONFIG2 register */
+ val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_CONFIG2);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_CONFIG2, val) ) {
+ return false;
+ }
+
+ /* Set bits in register to given value */
+ boost &= 0x03;//0b00000011;
+ boost = boost << 4;
+ val &= 0xCF;//0b11001111;
+ val |= boost;
+
+ /* Write register value back into CONFIG2 register */
+
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_CONFIG2, val)){//!wireWriteDataByte(APDS9960_CONFIG2, val) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Gets proximity gain compensation enable
+ *
+ * @return 1 if compensation is enabled. 0 if not. 0xFF on error.
+ */
+uint8_t glibr::getProxGainCompEnable()
+{
+ uint8_t val;
+
+ /* Read value from CONFIG3 register */
+ val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_CONFIG3);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_CONFIG3, val) ) {
+ return ERROR;
+ }
+
+ /* Shift and mask out PCMP bits */
+ val = (val >> 5) & 0x01;//0b00000001;
+
+ return val;
+}
+
+/**
+ * @brief Sets the proximity gain compensation enable
+ *
+ * @param[in] enable 1 to enable compensation. 0 to disable compensation.
+ * @return True if operation successful. False otherwise.
+ */
+ bool glibr::setProxGainCompEnable(uint8_t enable)
+{
+ uint8_t val;
+
+ /* Read value from CONFIG3 register */
+ val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_CONFIG3);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_CONFIG3, val) ) {
+ return false;
+ }
+
+ /* Set bits in register to given value */
+ enable &= 0x01;//0b00000001;
+ enable = enable << 5;
+ val &= 0xCF;//0b11011111;
+ val |= enable;
+
+ /* Write register value back into CONFIG3 register */
+
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_CONFIG3, val)){//!wireWriteDataByte(APDS9960_CONFIG3, val) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Gets the current mask for enabled/disabled proximity photodiodes
+ *
+ * 1 = disabled, 0 = enabled
+ * Bit Photodiode
+ * 3 UP
+ * 2 DOWN
+ * 1 LEFT
+ * 0 RIGHT
+ *
+ * @return Current proximity mask for photodiodes. 0xFF on error.
+ */
+uint8_t glibr::getProxPhotoMask()
+{
+ uint8_t val;
+
+ /* Read value from CONFIG3 register */
+ val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_CONFIG3);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_CONFIG3, val) ) {
+ return ERROR;
+ }
+
+ /* Mask out photodiode enable mask bits */
+ val &= 0x0F;//0b00001111;
+
+ return val;
+}
+
+/**
+ * @brief Sets the mask for enabling/disabling proximity photodiodes
+ *
+ * 1 = disabled, 0 = enabled
+ * Bit Photodiode
+ * 3 UP
+ * 2 DOWN
+ * 1 LEFT
+ * 0 RIGHT
+ *
+ * @param[in] mask 4-bit mask value
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::setProxPhotoMask(uint8_t mask)
+{
+ uint8_t val;
+
+ /* Read value from CONFIG3 register */
+ val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_CONFIG3);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_CONFIG3, val) ) {
+ return false;
+ }
+
+ /* Set bits in register to given value */
+ mask &= 0x0F;//0b00001111;
+ val &= 0xF0;//0b11110000;
+ val |= mask;
+
+ /* Write register value back into CONFIG3 register */
+ I2CwriteByte(APDS9960_I2C_ADDR, APDS9960_CONFIG3, val);
+ if( val == ERROR){//!wireWriteDataByte(APDS9960_CONFIG3, val) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Gets the entry proximity threshold for gesture sensing
+ *
+ * @return Current entry proximity threshold.
+ */
+uint8_t glibr::getGestureEnterThresh()
+{
+ uint8_t val;
+
+ /* Read value from GPENTH register */
+ val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GPENTH);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_GPENTH, val) ) {
+ val = 0;
+ }
+
+ return val;
+}
+
+/**
+ * @brief Sets the entry proximity threshold for gesture sensing
+ *
+ * @param[in] threshold proximity value needed to start gesture mode
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::setGestureEnterThresh(uint8_t threshold)
+{
+
+ if( I2CwriteByte(APDS9960_I2C_ADDR, APDS9960_GPENTH, threshold)){;//!wireWriteDataByte(APDS9960_GPENTH, threshold) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Gets the exit proximity threshold for gesture sensing
+ *
+ * @return Current exit proximity threshold.
+ */
+uint8_t glibr::getGestureExitThresh()
+{
+ uint8_t val;
+
+ /* Read value from GEXTH register */
+ val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GEXTH);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_GEXTH, val) ) {
+ val = 0;
+ }
+
+ return val;
+}
+
+/**
+ * @brief Sets the exit proximity threshold for gesture sensing
+ *
+ * @param[in] threshold proximity value needed to end gesture mode
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::setGestureExitThresh(uint8_t threshold)
+{
+ if( I2CwriteByte(APDS9960_I2C_ADDR, APDS9960_GEXTH, threshold)){//!wireWriteDataByte(APDS9960_GEXTH, threshold) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Gets the gain of the photodiode during gesture mode
+ *
+ * Value Gain
+ * 0 1x
+ * 1 2x
+ * 2 4x
+ * 3 8x
+ *
+ * @return the current photodiode gain. 0xFF on error.
+ */
+uint8_t glibr::getGestureGain()
+{
+ uint8_t val;
+
+ /* Read value from GCONF2 register */
+ val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GCONF2);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_GCONF2, val) ) {
+ return ERROR;
+ }
+
+ /* Shift and mask out GGAIN bits */
+ val = (val >> 5) & 0x03;//0b00000011;
+
+ return val;
+}
+
+/**
+ * @brief Sets the gain of the photodiode during gesture mode
+ *
+ * Value Gain
+ * 0 1x
+ * 1 2x
+ * 2 4x
+ * 3 8x
+ *
+ * @param[in] gain the value for the photodiode gain
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::setGestureGain(uint8_t gain)
+{
+ uint8_t val;
+
+ /* Read value from GCONF2 register */
+ val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GCONF2);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_GCONF2, val) ) {
+ return false;
+ }
+
+ /* Set bits in register to given value */
+ gain &= 0x03;//0b00000011;
+ gain = gain << 5;
+ val &= 0x9F;//0b10011111;
+ val |= gain;
+
+ /* Write register value back into GCONF2 register */
+ if( I2CwriteByte(APDS9960_I2C_ADDR, APDS9960_GCONF2, val)){//!wireWriteDataByte(APDS9960_GCONF2, val) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Gets the drive current of the LED during gesture mode
+ *
+ * Value LED Current
+ * 0 100 mA
+ * 1 50 mA
+ * 2 25 mA
+ * 3 12.5 mA
+ *
+ * @return the LED drive current value. 0xFF on error.
+ */
+uint8_t glibr::getGestureLEDDrive()
+{
+ uint8_t val;
+
+ /* Read value from GCONF2 register */
+ val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GCONF2);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_GCONF2, val) ) {
+ return ERROR;
+ }
+
+ /* Shift and mask out GLDRIVE bits */
+ val = (val >> 3) & 0x03;//0b00000011;
+
+ return val;
+}
+
+/**
+ * @brief Sets the LED drive current during gesture mode
+ *
+ * Value LED Current
+ * 0 100 mA
+ * 1 50 mA
+ * 2 25 mA
+ * 3 12.5 mA
+ *
+ * @param[in] drive the value for the LED drive current
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::setGestureLEDDrive(uint8_t drive)
+{
+ uint8_t val;
+
+ /* Read value from GCONF2 register */
+ val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GCONF2);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_GCONF2, val) ) {
+ return false;
+ }
+
+ /* Set bits in register to given value */
+ drive &= 0x03;//0b00000011;
+ drive = drive << 3;
+ val &= 0xE7;//0b11100111;
+ val |= drive;
+
+ /* Write register value back into GCONF2 register */
+ if( I2CwriteByte(APDS9960_I2C_ADDR, APDS9960_GCONF2, val)){//!wireWriteDataByte(APDS9960_GCONF2, val) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Gets the time in low power mode between gesture detections
+ *
+ * Value Wait time
+ * 0 0 ms
+ * 1 2.8 ms
+ * 2 5.6 ms
+ * 3 8.4 ms
+ * 4 14.0 ms
+ * 5 22.4 ms
+ * 6 30.8 ms
+ * 7 39.2 ms
+ *
+ * @return the current wait time between gestures. 0xFF on error.
+ */
+uint8_t glibr::getGestureWaitTime()
+{
+ uint8_t val;
+
+ /* Read value from GCONF2 register */
+ val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GCONF2);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_GCONF2, val) ) {
+ return ERROR;
+ }
+
+ /* Mask out GWTIME bits */
+ val &= 0x07;//0b00000111;
+
+ return val;
+}
+
+/*
+*
+*
+*
+*LEFT Off HERE AT 3:47PM On 3/6/15
+*
+*
+*
+*
+*/
+
+
+/**
+ * @brief Sets the time in low power mode between gesture detections
+ *
+ * Value Wait time
+ * 0 0 ms
+ * 1 2.8 ms
+ * 2 5.6 ms
+ * 3 8.4 ms
+ * 4 14.0 ms
+ * 5 22.4 ms
+ * 6 30.8 ms
+ * 7 39.2 ms
+ *
+ * @param[in] the value for the wait time
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::setGestureWaitTime(uint8_t time)
+{
+ uint8_t val;
+
+ /* Read value from GCONF2 register */
+ val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GCONF2);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_GCONF2, val) ) {
+ return false;
+ }
+ /* if( !wireReadDataByte(APDS9960_GCONF2, val) ) {
+ return false;
+ } */
+
+ /* Set bits in register to given value */
+ time &= 0x07;//0b00000111;
+ val &= 0xF8;//0b11111000;
+ val |= time;
+
+ /* Write register value back into GCONF2 register */
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_GCONF2,val)){//!wireWriteDataByte(APDS9960_GCONF2, val) ) {
+ return false;
+ }
+ /*if( !wireWriteDataByte(APDS9960_GCONF2, val) ) {
+ return false;
+ }*/
+ return true;
+}
+
+/**
+ * @brief Gets the low threshold for ambient light interrupts
+ *
+ * @param[out] threshold current low threshold stored on the APDS-9960
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::getLightIntLowThreshold(uint16_t &threshold)
+{
+ uint8_t val_byte;
+ threshold = 0;
+
+ /* Read value from ambient light low threshold, low byte register */
+ val_byte = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_AILTL);
+ if( val_byte == ERROR){//!wireReadDataByte(APDS9960_AILTL, val_byte) ) {
+ return false;
+ }
+ threshold = val_byte;
+
+ /* Read value from ambient light low threshold, high byte register */
+ val_byte = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_AILTH);
+ if( val_byte == ERROR){//!wireReadDataByte(APDS9960_AILTH, val_byte) ) {
+ return false;
+ }
+ threshold = threshold + ((uint16_t)val_byte << 8);
+
+ return true;
+}
+
+/**
+ * @brief Sets the low threshold for ambient light interrupts
+ *
+ * @param[in] threshold low threshold value for interrupt to trigger
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::setLightIntLowThreshold(uint16_t threshold)
+{
+ uint8_t val_low;
+ uint8_t val_high;
+
+ /* Break 16-bit threshold into 2 8-bit values */
+ val_low = threshold & 0x00FF;
+ val_high = (threshold & 0xFF00) >> 8;
+
+ /* Write low byte */
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_AILTL,val_low)){//!wireWriteDataByte(APDS9960_AILTL, val_low) ) {
+ return false;
+ }
+
+ /* Write high byte */
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_AILTH,val_high)){//!wireWriteDataByte(APDS9960_AILTH, val_high) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Gets the high threshold for ambient light interrupts
+ *
+ * @param[out] threshold current low threshold stored on the APDS-9960
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::getLightIntHighThreshold(uint16_t &threshold)
+{
+ uint8_t val_byte;
+ threshold = 0;
+
+ /* Read value from ambient light high threshold, low byte register */
+ val_byte = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_AIHTL);
+ if( val_byte == ERROR){//!wireReadDataByte(APDS9960_AIHTL, val_byte) ) {
+ return false;
+ }
+ threshold = val_byte;
+
+ /* Read value from ambient light high threshold, high byte register */
+ val_byte = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_AIHTH);
+ if( val_byte == ERROR){//!wireReadDataByte(APDS9960_AIHTH, val_byte) ) {
+ return false;
+ }
+ threshold = threshold + ((uint16_t)val_byte << 8);
+
+ return true;
+}
+
+/**
+ * @brief Sets the high threshold for ambient light interrupts
+ *
+ * @param[in] threshold high threshold value for interrupt to trigger
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::setLightIntHighThreshold(uint16_t threshold)
+{
+ uint8_t val_low;
+ uint8_t val_high;
+
+ /* Break 16-bit threshold into 2 8-bit values */
+ val_low = threshold & 0x00FF;
+ val_high = (threshold & 0xFF00) >> 8;
+
+ /* Write low byte */
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_AIHTL,val_low)){//!wireWriteDataByte(APDS9960_AIHTL, val_low) ) {
+ return false;
+ }
+
+ /* Write high byte */
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_AIHTH,val_high)){//!wireWriteDataByte(APDS9960_AIHTH, val_high) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Gets the low threshold for proximity interrupts
+ *
+ * @param[out] threshold current low threshold stored on the APDS-9960
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::getProximityIntLowThreshold(uint8_t &threshold)
+{
+ threshold = 0;
+
+ /* Read value from proximity low threshold register */
+ threshold = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_PILT);
+ if( threshold == ERROR){//!wireReadDataByte(APDS9960_PILT, threshold) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Sets the low threshold for proximity interrupts
+ *
+ * @param[in] threshold low threshold value for interrupt to trigger
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::setProximityIntLowThreshold(uint8_t threshold)
+{
+
+ /* Write threshold value to register */
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_PILT,threshold)){//!wireWriteDataByte(APDS9960_PILT, threshold) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Gets the high threshold for proximity interrupts
+ *
+ * @param[out] threshold current low threshold stored on the APDS-9960
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::getProximityIntHighThreshold(uint8_t &threshold)
+{
+ threshold = 0;
+
+ /* Read value from proximity low threshold register */
+ threshold = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_PIHT);
+ if( threshold == ERROR){//!wireReadDataByte(APDS9960_PIHT, threshold) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Sets the high threshold for proximity interrupts
+ *
+ * @param[in] threshold high threshold value for interrupt to trigger
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::setProximityIntHighThreshold(uint8_t threshold)
+{
+
+ /* Write threshold value to register */
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_PIHT,threshold)){//!wireWriteDataByte(APDS9960_PIHT, threshold) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Gets if ambient light interrupts are enabled or not
+ *
+ * @return 1 if interrupts are enabled, 0 if not. 0xFF on error.
+ */
+uint8_t glibr::getAmbientLightIntEnable()
+{
+ uint8_t val;
+
+ /* Read value from ENABLE register */
+ val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_ENABLE);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_ENABLE, val) ) {
+ return ERROR;
+ }
+
+ /* Shift and mask out AIEN bit */
+ val = (val >> 4) & 0x01;//0b00000001;
+
+ return val;
+}
+
+/**
+ * @brief Turns ambient light interrupts on or off
+ *
+ * @param[in] enable 1 to enable interrupts, 0 to turn them off
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::setAmbientLightIntEnable(uint8_t enable)
+{
+ uint8_t val;
+
+ /* Read value from ENABLE register */
+ val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_ENABLE);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_ENABLE, val) ) {
+ return false;
+ }
+
+ /* Set bits in register to given value */
+ enable &= 0x01;//0b00000001;
+ enable = enable << 4;
+ val &= 0xEF;//0b11101111;
+ val |= enable;
+
+ /* Write register value back into ENABLE register */
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_ENABLE,val)){//!wireWriteDataByte(APDS9960_ENABLE, val) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Gets if proximity interrupts are enabled or not
+ *
+ * @return 1 if interrupts are enabled, 0 if not. 0xFF on error.
+ */
+uint8_t glibr::getProximityIntEnable()
+{
+ uint8_t val;
+
+ /* Read value from ENABLE register */
+ val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_ENABLE);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_ENABLE, val) ) {
+ return ERROR;
+ }
+
+ /* Shift and mask out PIEN bit */
+ val = (val >> 5) & 0x01;//0b00000001;
+
+ return val;
+}
+
+/**
+ * @brief Turns proximity interrupts on or off
+ *
+ * @param[in] enable 1 to enable interrupts, 0 to turn them off
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::setProximityIntEnable(uint8_t enable)
+{
+ uint8_t val;
+
+ /* Read value from ENABLE register */
+ val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_ENABLE);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_ENABLE, val) ) {
+ return false;
+ }
+
+ /* Set bits in register to given value */
+ enable &= 0x01;//0b00000001;
+ enable = enable << 5;
+ val &= 0xDF;//0b11011111;
+ val |= enable;
+
+ /* Write register value back into ENABLE register */
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_ENABLE,val)){//!wireWriteDataByte(APDS9960_ENABLE, val) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Gets if gesture interrupts are enabled or not
+ *
+ * @return 1 if interrupts are enabled, 0 if not. 0xFF on error.
+ */
+uint8_t glibr::getGestureIntEnable()
+{
+ uint8_t val;
+
+ /* Read value from GCONF4 register */
+ val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GCONF4);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_GCONF4, val) ) {
+ return ERROR;
+ }
+
+ /* Shift and mask out GIEN bit */
+ val = (val >> 1) & 0x01;//0b00000001;
+
+ return val;
+}
+
+/**
+ * @brief Turns gesture-related interrupts on or off
+ *
+ * @param[in] enable 1 to enable interrupts, 0 to turn them off
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::setGestureIntEnable(uint8_t enable)
+{
+ uint8_t val;
+
+ /* Read value from GCONF4 register */
+ val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GCONF4);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_GCONF4, val) ) {
+ return false;
+ }
+
+ /* Set bits in register to given value */
+ enable &= 0x01;//0b00000001;
+ enable = enable << 1;
+ val &= 0xFD;//0b11111101;
+ val |= enable;
+
+ /* Write register value back into GCONF4 register */
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_GCONF4,val)){//!wireWriteDataByte(APDS9960_GCONF4, val) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Clears the ambient light interrupt
+ *
+ * @return True if operation completed successfully. False otherwise.
+ */
+bool glibr::clearAmbientLightInt()
+{
+ uint8_t throwaway;
+ throwaway = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_AICLEAR);
+ if( throwaway == ERROR){//!wireReadDataByte(APDS9960_AICLEAR, throwaway) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Clears the proximity interrupt
+ *
+ * @return True if operation completed successfully. False otherwise.
+ */
+bool glibr::clearProximityInt()
+{
+ uint8_t throwaway;
+ throwaway = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_PICLEAR);
+ if( throwaway == ERROR){//!wireReadDataByte(APDS9960_PICLEAR, throwaway) ) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Tells if the gesture state machine is currently running
+ *
+ * @return 1 if gesture state machine is running, 0 if not. 0xFF on error.
+ */
+uint8_t glibr::getGestureMode()
+{
+ uint8_t val;
+
+ /* Read value from GCONF4 register */
+ val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GCONF4);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_GCONF4, val) ) {
+ return ERROR;
+ }
+
+ /* Mask out GMODE bit */
+ val &= 0x01;//0b00000001;
+
+ return val;
+}
+
+/**
+ * @brief Tells the state machine to either enter or exit gesture state machine
+ *
+ * @param[in] mode 1 to enter gesture state machine, 0 to exit.
+ * @return True if operation successful. False otherwise.
+ */
+bool glibr::setGestureMode(uint8_t mode)
+{
+ uint8_t val;
+
+ /* Read value from GCONF4 register */
+ val = I2CreadByte(APDS9960_I2C_ADDR, APDS9960_GCONF4);
+ if( val == ERROR){//!wireReadDataByte(APDS9960_GCONF4, val) ) {
+ return false;
+ }
+
+ /* Set bits in register to given value */
+ mode &= 0x01;//0b00000001;
+ val &= 0xFE;//0b11111110;
+ val |= mode;
+
+ /* Write register value back into GCONF4 register */
+ if( I2CwriteByte(APDS9960_I2C_ADDR,APDS9960_GCONF4,val)){//!wireWriteDataByte(APDS9960_GCONF4, val) ) {
+ return false;
+ }
+
+ return true;
+}
+
+
+
+
+
+int glibr::I2CwriteByte(char address, char subAddress, char data)
+{
+ int ret;
+ char cmd[2] = {subAddress, data};
+ ret=i2c.write(address<<1, cmd, 2); //if ret is 1, then not acked.
+ return ret;
+}
+
+
+
+uint8_t glibr::I2CreadByte(char address, char subAddress)
+{
+ char data; // store the register data
+
+ if(i2c.write(address<<1, &subAddress, 1, true)){
+ return ERROR; //7 bit //not acked
+ }
+ if(i2c.read(address<<1, &data, 1)){ /////CHANGED THIS NEED TO TEST.
+ return ERROR;
+ }
+
+
+ //i2c.read(address<<1, &data, 1);
+ return data;
+
+}
+
+
+// * @brief Reads a block (array) of bytes from the I2C device and register
+// *
+// * @param[in] reg the register to read from
+// * @param[out] val pointer to the beginning of the data
+// * @param[in] len number of bytes to read
+// * @return Number of bytes read. -1 on read error.
+// */
+int glibr::I2CReadDataBlock(char address, char subAddress, char *data, unsigned int len)
+{
+ // unsigned char i = 0;
+
+ /* Indicate which register we want to read from */
+
+ if(i2c.write(address<<1, &subAddress, 1, true)){
+ return -1; //7 bit //not acked
+ }
+
+ /* Read block data */
+
+ if(i2c.read(address<<1, data, len)){
+ return -1;
+ }
+
+ return 1;
+ //Wire.requestFrom(APDS9960_I2C_ADDR, len);
+ /*while (Wire.available()) {
+ if (i >= len) {
+ return -1;
+ }
+ val[i] = Wire.read();
+ i++;
+ }*/
+}
\ No newline at end of file
diff -r 35738c77d454 -r 9e51de1050a1 APDS_9960/glibr.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/APDS_9960/glibr.h Sat Nov 25 03:36:58 2017 +0000
@@ -0,0 +1,334 @@
+#include "mbed.h"
+
+
+#define DEBUG 0
+
+/* APDS-9960 I2C address */
+#define APDS9960_I2C_ADDR 0x39
+
+/* Gesture parameters */
+#define GESTURE_THRESHOLD_OUT 10
+#define GESTURE_SENSITIVITY_1 50
+#define GESTURE_SENSITIVITY_2 20
+
+/* Error code for returned values */
+#define ERROR 0xFF
+
+/* Acceptable device IDs */
+#define APDS9960_ID_1 0xAB
+#define APDS9960_ID_2 0x9C
+#define APDS9960_ID_3 0xA8
+
+/* Misc parameters */
+#define FIFO_PAUSE_TIME 0.30 // Wait period (ms) between FIFO reads
+
+/* APDS-9960 register addresses */
+#define APDS9960_ENABLE 0x80
+#define APDS9960_ATIME 0x81
+#define APDS9960_WTIME 0x83
+#define APDS9960_AILTL 0x84
+#define APDS9960_AILTH 0x85
+#define APDS9960_AIHTL 0x86
+#define APDS9960_AIHTH 0x87
+#define APDS9960_PILT 0x89
+#define APDS9960_PIHT 0x8B
+#define APDS9960_PERS 0x8C
+#define APDS9960_CONFIG1 0x8D
+#define APDS9960_PPULSE 0x8E
+#define APDS9960_CONTROL 0x8F
+#define APDS9960_CONFIG2 0x90
+#define APDS9960_ID 0x92
+#define APDS9960_STATUS 0x93
+#define APDS9960_CDATAL 0x94
+#define APDS9960_CDATAH 0x95
+#define APDS9960_RDATAL 0x96
+#define APDS9960_RDATAH 0x97
+#define APDS9960_GDATAL 0x98
+#define APDS9960_GDATAH 0x99
+#define APDS9960_BDATAL 0x9A
+#define APDS9960_BDATAH 0x9B
+#define APDS9960_PDATA 0x9C
+#define APDS9960_POFFSET_UR 0x9D
+#define APDS9960_POFFSET_DL 0x9E
+#define APDS9960_CONFIG3 0x9F
+#define APDS9960_GPENTH 0xA0
+#define APDS9960_GEXTH 0xA1
+#define APDS9960_GCONF1 0xA2
+#define APDS9960_GCONF2 0xA3
+#define APDS9960_GOFFSET_U 0xA4
+#define APDS9960_GOFFSET_D 0xA5
+#define APDS9960_GOFFSET_L 0xA7
+#define APDS9960_GOFFSET_R 0xA9
+#define APDS9960_GPULSE 0xA6
+#define APDS9960_GCONF3 0xAA
+#define APDS9960_GCONF4 0xAB
+#define APDS9960_GFLVL 0xAE
+#define APDS9960_GSTATUS 0xAF
+#define APDS9960_IFORCE 0xE4
+#define APDS9960_PICLEAR 0xE5
+#define APDS9960_CICLEAR 0xE6
+#define APDS9960_AICLEAR 0xE7
+#define APDS9960_GFIFO_U 0xFC
+#define APDS9960_GFIFO_D 0xFD
+#define APDS9960_GFIFO_L 0xFE
+#define APDS9960_GFIFO_R 0xFF
+
+/* Bit fields */
+#define APDS9960_PON 0x01
+#define APDS9960_AEN 0x02
+#define APDS9960_PEN 0x04
+#define APDS9960_WEN 0x08
+#define APSD9960_AIEN 0x10
+#define APDS9960_PIEN 0x20
+#define APDS9960_GEN 0x40
+#define APDS9960_GVALID 0x01
+
+/* On/Off definitions */
+#define Off 0
+#define On 1
+
+/* Acceptable parameters for setMode */
+#define Power 0
+#define AMBIENT_LIGHT 1
+#define PROXIMITY 2
+#define WAIT 3
+#define AMBIENT_LIGHT_INT 4
+#define PROXIMITY_INT 5
+#define GESTURE 6
+#define ALL 7
+
+/* LED Drive values */
+#define LED_DRIVE_100MA 0
+#define LED_DRIVE_50MA 1
+#define LED_DRIVE_25MA 2
+#define LED_DRIVE_12_5MA 3
+
+/* Proximity Gain (PGAIN) values */
+#define PGAIN_1X 0
+#define PGAIN_2X 1
+#define PGAIN_4X 2
+#define PGAIN_8X 3
+
+/* ALS Gain (AGAIN) values */
+#define AGAIN_1X 0
+#define AGAIN_4X 1
+#define AGAIN_16X 2
+#define AGAIN_64X 3
+
+/* Gesture Gain (GGAIN) values */
+#define GGAIN_1X 0
+#define GGAIN_2X 1
+#define GGAIN_4X 2
+#define GGAIN_8X 3
+
+/* LED Boost values */
+#define LED_BOOST_100 0
+#define LED_BOOST_150 1
+#define LED_BOOST_200 2
+#define LED_BOOST_300 3
+
+/* Gesture wait time values */
+#define GWTIME_0MS 0
+#define GWTIME_2_8MS 1
+#define GWTIME_5_6MS 2
+#define GWTIME_8_4MS 3
+#define GWTIME_14_0MS 4
+#define GWTIME_22_4MS 5
+#define GWTIME_30_8MS 6
+#define GWTIME_39_2MS 7
+
+/* Default values */
+#define DEFAULT_ATIME 219 // 103ms
+#define DEFAULT_WTIME 246 // 27ms
+#define DEFAULT_PROX_PPULSE 0x87 // 16us, 8 pulses
+#define DEFAULT_GESTURE_PPULSE 0x89 // 16us, 10 pulses
+#define DEFAULT_POFFSET_UR 0 // 0 offset
+#define DEFAULT_POFFSET_DL 0 // 0 offset
+#define DEFAULT_CONFIG1 0x60 // No 12x wait (WTIME) factor
+#define DEFAULT_LDRIVE LED_DRIVE_100MA
+#define DEFAULT_PGAIN PGAIN_4X
+#define DEFAULT_AGAIN AGAIN_4X
+#define DEFAULT_PILT 0 // Low proximity threshold
+#define DEFAULT_PIHT 50 // High proximity threshold
+#define DEFAULT_AILT 0xFFFF // Force interrupt for calibration
+#define DEFAULT_AIHT 0
+#define DEFAULT_PERS 0x11 // 2 consecutive prox or ALS for int.
+#define DEFAULT_CONFIG2 0x01 // No saturation interrupts or LED boost
+#define DEFAULT_CONFIG3 0 // Enable all photodiodes, no SAI
+#define DEFAULT_GPENTH 40 // Threshold for entering gesture mode
+#define DEFAULT_GEXTH 30 // Threshold for exiting gesture mode
+#define DEFAULT_GCONF1 0x40 // 4 gesture events for int., 1 for exit
+#define DEFAULT_GGAIN GGAIN_4X
+#define DEFAULT_GLDRIVE LED_DRIVE_100MA
+#define DEFAULT_GWTIME GWTIME_2_8MS
+#define DEFAULT_GOFFSET 0 // No offset scaling for gesture mode
+#define DEFAULT_GPULSE 0xC9 // 32us, 10 pulses
+#define DEFAULT_GCONF3 0 // All photodiodes active during gesture
+#define DEFAULT_GIEN 0 // Disable gesture interrupts
+
+/* Direction definitions */
+enum {
+ DIR_NONE,
+ DIR_LEFT,
+ DIR_RIGHT,
+ DIR_UP,
+ DIR_DOWN,
+ DIR_NEAR,
+ DIR_FAR,
+ DIR_ALL
+};
+
+/* State definitions */
+enum {
+ NA_STATE,
+ NEAR_STATE,
+ FAR_STATE,
+ ALL_STATE
+};
+
+/* Container for gesture data */
+typedef struct gesture_data_type {
+ uint8_t u_data[32];
+ uint8_t d_data[32];
+ uint8_t l_data[32];
+ uint8_t r_data[32];
+ uint8_t sindex;
+ uint8_t total_gestures;
+ uint8_t in_threshold;
+ uint8_t out_threshold;
+} gesture_data_type;
+
+
+class glibr{
+
+ public:
+
+
+ glibr(PinName sda, PinName scl); //constructor
+ ~glibr();
+
+
+ bool ginit();
+ /* Initialization methods */
+
+ uint8_t getMode();
+ bool setMode(uint8_t mode, uint8_t enable);
+
+ /* Turn the APDS-9960 on and off */
+ bool enablePower();
+ bool disablePower();
+
+ /* Enable or disable specific sensors */
+ bool enableLightSensor(bool interrupts = false);
+ bool disableLightSensor();
+ bool enableProximitySensor(bool interrupts = false);
+ bool disableProximitySensor();
+ bool enableGestureSensor(bool interrupts = true);
+ bool disableGestureSensor();
+
+ /* LED drive strength control */
+ uint8_t getLEDDrive();
+ bool setLEDDrive(uint8_t drive);
+ uint8_t getGestureLEDDrive();
+ bool setGestureLEDDrive(uint8_t drive);
+
+ /* Gain control */
+ uint8_t getAmbientLightGain();
+ bool setAmbientLightGain(uint8_t gain);
+ uint8_t getProximityGain();
+ bool setProximityGain(uint8_t gain);
+ uint8_t getGestureGain();
+ bool setGestureGain(uint8_t gain);
+
+ /* Get and set light interrupt thresholds */
+ bool getLightIntLowThreshold(uint16_t &threshold);
+ bool setLightIntLowThreshold(uint16_t threshold);
+ bool getLightIntHighThreshold(uint16_t &threshold);
+ bool setLightIntHighThreshold(uint16_t threshold);
+
+ /* Get and set proximity interrupt thresholds */
+ bool getProximityIntLowThreshold(uint8_t &threshold);
+ bool setProximityIntLowThreshold(uint8_t threshold);
+ bool getProximityIntHighThreshold(uint8_t &threshold);
+ bool setProximityIntHighThreshold(uint8_t threshold);
+
+ /* Get and set interrupt enables */
+ uint8_t getAmbientLightIntEnable();
+ bool setAmbientLightIntEnable(uint8_t enable);
+ uint8_t getProximityIntEnable();
+ bool setProximityIntEnable(uint8_t enable);
+ uint8_t getGestureIntEnable();
+ bool setGestureIntEnable(uint8_t enable);
+
+ /* Clear interrupts */
+ bool clearAmbientLightInt();
+ bool clearProximityInt();
+
+ /* Ambient light methods */
+ bool readAmbientLight(uint16_t &val);
+ bool readRedLight(uint16_t &val);
+ bool readGreenLight(uint16_t &val);
+ bool readBlueLight(uint16_t &val);
+
+ /* Proximity methods */
+ bool readProximity(uint8_t &val);
+
+ /* Gesture methods */
+ bool isGestureAvailable();
+ int readGesture();
+
+ private:
+ /* Gesture processing */
+ void resetGestureParameters();
+ bool processGestureData();
+ bool decodeGesture();
+
+ /* Proximity Interrupt Threshold */
+ uint8_t getProxIntLowThresh();
+ bool setProxIntLowThresh(uint8_t threshold);
+ uint8_t getProxIntHighThresh();
+ bool setProxIntHighThresh(uint8_t threshold);
+
+ /* LED Boost Control */
+ uint8_t getLEDBoost();
+ bool setLEDBoost(uint8_t boost);
+
+ /* Proximity photodiode select */
+ uint8_t getProxGainCompEnable();
+ bool setProxGainCompEnable(uint8_t enable);
+ uint8_t getProxPhotoMask();
+ bool setProxPhotoMask(uint8_t mask);
+
+ /* Gesture threshold control */
+ uint8_t getGestureEnterThresh();
+ bool setGestureEnterThresh(uint8_t threshold);
+ uint8_t getGestureExitThresh();
+ bool setGestureExitThresh(uint8_t threshold);
+
+ /* Gesture LED, gain, and time control */
+ uint8_t getGestureWaitTime();
+ bool setGestureWaitTime(uint8_t time);
+
+ /* Gesture mode */
+ uint8_t getGestureMode();
+ bool setGestureMode(uint8_t mode);
+
+ /* Members */
+ gesture_data_type gesture_data_; //instanciation from struct
+ int gesture_ud_delta_;
+ int gesture_lr_delta_;
+ int gesture_ud_count_;
+ int gesture_lr_count_;
+ int gesture_near_count_;
+ int gesture_far_count_;
+ int gesture_state_;
+ int gesture_motion_;
+
+
+
+ uint8_t I2CreadByte(char address, char subAddress);
+ int I2CwriteByte(char address, char subAddress, char data); //if return value is 1, then ack not sent
+ int I2CReadDataBlock(char address, char subAddress, char *data, unsigned int len); //reads array of bytes
+ I2C i2c;
+
+};
diff -r 35738c77d454 -r 9e51de1050a1 AutomationElement/AutomationElements.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/AutomationElement/AutomationElements.h Sat Nov 25 03:36:58 2017 +0000 @@ -0,0 +1,12 @@ +#ifndef AUTOMATION_ELEMENTS_H +#define AUTOMATION_ELEMENTS_H + +#include "DT1.h" +#include "I.h" +#include "PDT1.h" +#include "PI1.h" +#include "PT1.h" +#include "PT2.h" +#include "PT2cc.h" + +#endif // AUTOMATION_ELEMENTS_H \ No newline at end of file
diff -r 35738c77d454 -r 9e51de1050a1 AutomationElement/DT1.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AutomationElement/DT1.cpp Sat Nov 25 03:36:58 2017 +0000
@@ -0,0 +1,34 @@
+#include "DT1.h"
+
+DT1::DT1() {
+ u = y = u_p1 = y_p1 = 0;
+ setParameters(1, 0.5, 0.1);
+}
+
+DT1::DT1(double T_1_, double T_D_, double T_d_) {
+ u = y = u_p1 = y_p1 = 0;
+ setParameters(T_1_, T_D_, T_d_);
+}
+
+void DT1::setParameters(double T_1_, double T_D_, double T_d_) {
+ T_1 = T_1_;
+ T_D = T_D_;
+ if (T_d_ > 0) // only positive sample time values allowed
+ T_d = T_d_;
+ else
+ T_d = 0.1;
+ a_1 = -exp(-T_d/T_1);
+ b_0 = T_D / T_1;
+ b_1 = -b_0;
+}
+
+void DT1::in(double u_) {
+ u = u_;
+}
+
+double DT1::out() {
+ y = -a_1*y_p1 + b_0*u + b_1*u_p1;
+ y_p1 = y;
+ u_p1 = u;
+ return y;
+}
\ No newline at end of file
diff -r 35738c77d454 -r 9e51de1050a1 AutomationElement/DT1.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AutomationElement/DT1.h Sat Nov 25 03:36:58 2017 +0000
@@ -0,0 +1,32 @@
+#ifndef DT1_H
+#define DT1_H
+
+#include "mbed.h"
+
+/** Transfer function of a DT1 form.
+ * G(s) = Y(s) / U(s) = T_D s / (1 + T_1 s)
+ *
+ * Author(s): TVZ Mechatronics Team
+ *
+ */
+class DT1 {
+ public:
+ /** Default constructor.
+ * T_1 = 1, T_D = 0.5, sampleTime = 0.1
+ */
+ DT1();
+ /** DT1 transfer function parameters and sample time in seconds. */
+ DT1(double T_1, double T_D, double sampleTime);
+ /** Update DT1 transfer function parameters and sample time. */
+ void setParameters(double T_1, double T_D, double sampleTime);
+ /** Calculate the output y. */
+ double out();
+ /** Set the input u. */
+ void in(double u);
+ private:
+ double y, y_p1;
+ double u, u_p1;
+ double T_1, T_D, T_d, a_1, b_0, b_1;
+};
+
+#endif // DT1_H
\ No newline at end of file
diff -r 35738c77d454 -r 9e51de1050a1 AutomationElement/I.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AutomationElement/I.cpp Sat Nov 25 03:36:58 2017 +0000
@@ -0,0 +1,30 @@
+#include "I.h"
+
+I::I() {
+ u = y = u_p1 = y_p1 = 0;
+ setParameters(1, 0.1);
+}
+
+I::I(double K_I_, double T_d_) {
+ u = y = u_p1 = y_p1 = 0;
+ setParameters(K_I_, T_d_);
+}
+
+void I::setParameters(double K_I_, double T_d_) {
+ if (T_d_ > 0)
+ T_d = T_d_;
+ else
+ T_d = 0.1;
+ K_I = K_I_;
+}
+
+double I::out() {
+ y = y_p1 + K_I * T_d * u_p1;
+ y_p1 = y;
+ u_p1 = u;
+ return y;
+}
+
+void I::in(double u_) {
+ u = u_;
+}
\ No newline at end of file
diff -r 35738c77d454 -r 9e51de1050a1 AutomationElement/I.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AutomationElement/I.h Sat Nov 25 03:36:58 2017 +0000
@@ -0,0 +1,30 @@
+#ifndef I_H
+#define I_H
+
+#include "mbed.h"
+
+/** Transfer function of an integrator.
+ * G(s) = Y(s) / U(s) = K_I / s
+ *
+ * Author(s): TVZ Mechatronics Team
+ *
+ */
+class I {
+ public:
+ /** Default constructor.
+ * K_I = 1, sampleTime = 0.1
+ */
+ I();
+ /** Integrator gain and sample time in seconds. */
+ I(double K_I, double sampleTime);
+ /** Update integrator gain and sample time. */
+ void setParameters(double K_I, double sampleTime);
+ /** Calculate the output y. */
+ double out();
+ /** Set the input u. */
+ void in(double u);
+ private:
+ double K_I, T_d, y, y_p1, u, u_p1;
+};
+
+#endif // I_H
\ No newline at end of file
diff -r 35738c77d454 -r 9e51de1050a1 AutomationElement/PDT1.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AutomationElement/PDT1.cpp Sat Nov 25 03:36:58 2017 +0000
@@ -0,0 +1,35 @@
+#include "PDT1.h"
+
+PDT1::PDT1() {
+ u = y = u_p1 = y_p1 = 0;
+ setParameters(1, 1, 0.5, 0.1);
+}
+
+PDT1::PDT1(double K_, double T_1_, double T_D_, double T_d_) {
+ u = y = u_p1 = y_p1 = 0;
+ setParameters(K_, T_1_, T_D_, T_d_);
+}
+
+void PDT1::setParameters(double K_, double T_1_, double T_D_, double T_d_) {
+ K = K_;
+ T_1 = T_1_;
+ T_D = T_D_;
+ if (T_d_ > 0) // only positive sample time values allowed
+ T_d = T_d_;
+ else
+ T_d = 0.1;
+ a_1 = -exp(-T_d/T_1);
+ b_0 = K * T_D / T_1;
+ b_1 = K * (1 + a_1) - b_0;
+}
+
+void PDT1::in(double u_) {
+ u = u_;
+}
+
+double PDT1::out() {
+ y = -a_1*y_p1 + b_0*u + b_1*u_p1;
+ y_p1 = y;
+ u_p1 = u;
+ return y;
+}
\ No newline at end of file
diff -r 35738c77d454 -r 9e51de1050a1 AutomationElement/PDT1.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AutomationElement/PDT1.h Sat Nov 25 03:36:58 2017 +0000
@@ -0,0 +1,32 @@
+#ifndef PDT1_H
+#define PDT1_H
+
+#include "mbed.h"
+
+/** Transfer function of a PDT1 form.
+ * G(s) = Y(s) / U(s) = K * (1 + T_D s) / (1 + T_1 s)
+ *
+ * Author(s): TVZ Mechatronics Team
+ *
+ */
+class PDT1 {
+ public:
+ /** Default constructor.
+ * K = 1, T_1 = 1, T_D = 0.5, sampleTime = 0.1
+ */
+ PDT1();
+ /** PDT1 transfer function parameters and sample time in seconds. */
+ PDT1(double K, double T_1, double T_D, double sampleTime);
+ /** Update PDT1 transfer function parameters and sample time. */
+ void setParameters(double K, double T_1, double T_D, double sampleTime);
+ /** Calculate the output y. */
+ double out();
+ /** Set the input u. */
+ void in(double u);
+ private:
+ double y, y_p1;
+ double u, u_p1;
+ double K, T_1, T_D, T_d, a_1, b_0, b_1;
+};
+
+#endif // PDT1_H
\ No newline at end of file
diff -r 35738c77d454 -r 9e51de1050a1 AutomationElement/PI1.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AutomationElement/PI1.cpp Sat Nov 25 03:36:58 2017 +0000
@@ -0,0 +1,72 @@
+#include "PI1.h"
+
+PI1::PI1() {
+ u = y = u_p1 = y_p1 = 0;
+ setParameters(1, 1, 0.1);
+ setOutputLimits(0, 1);
+ setOperationMode(automatic);
+}
+
+PI1::PI1(double K_R_, double T_I_, double T_d_) {
+ u = y = u_p1 = y_p1 = 0;
+ setParameters(K_R_, T_I_, T_d_);
+ setOutputLimits(0, 1);
+ setOperationMode(automatic);
+}
+
+void PI1::setParameters(double K_R_, double T_I_, double T_d_) {
+ if (T_d_ > 0)
+ T_d = T_d_;
+ else
+ T_d = 0.1;
+ if (T_I_ > 0)
+ T_I = T_I_;
+ else
+ T_I = 1;
+ K_R = K_R_;
+}
+
+void PI1::setOutputLimits(double lowerOutputLimit, double upperOutputLimit) {
+ if (upperOutputLimit > lowerOutputLimit) {
+ y_min = lowerOutputLimit;
+ y_max = upperOutputLimit;
+ }
+}
+
+void PI1::setOperationMode(Mode newOperationMode) {
+ // Bumpless transfer from manual to automatic mode
+ if (operationMode == manual && newOperationMode == automatic) {
+ y = y_p1 = y_manual;
+ u = u_p1 = 0;
+ }
+ // Bumpless transfer from automatic to manual mode
+ else if (operationMode == automatic && newOperationMode == manual) {
+ y_manual = y;
+ }
+ operationMode = newOperationMode;
+}
+
+void PI1::setOutputManually(double u_C) {
+ y_manual = u_C;
+}
+
+double PI1::out() {
+ if (operationMode == automatic) {
+ y = y_p1 + K_R * u + K_R * (T_d / T_I - 1) * u_p1;
+ } else if (operationMode == manual) {
+ y = y_manual;
+ }
+
+ if (y > y_max)
+ y = y_max;
+ else if (y < y_min)
+ y = y_min;
+
+ y_p1 = y;
+ u_p1 = u;
+ return y;
+}
+
+void PI1::in(double u_) {
+ u = u_;
+}
\ No newline at end of file
diff -r 35738c77d454 -r 9e51de1050a1 AutomationElement/PI1.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AutomationElement/PI1.h Sat Nov 25 03:36:58 2017 +0000
@@ -0,0 +1,42 @@
+#ifndef PI1_H
+#define PI1_H
+
+#include "mbed.h"
+
+/** Data type for choosing the operation mode of the controller ("automatic" or "manual"). */
+enum Mode {automatic, manual};
+
+/** Transfer function of a PI controller with output limitation, anti-windup and bumpless automatic and manual mode.
+ * G_C(s) = U_C(s) / E(s) = K_C ( 1 + 1 / (T_I s) ) = K_C (1 + T_I s) / (T_I s)
+ *
+ * K_C is the controller gain, and T_I is the controller integral time constant in seconds.
+ *
+ * Author(s): TVZ Mechatronics Team
+ *
+ */
+class PI1 {
+ public:
+ /** Default constructor.
+ * K_C = 1, T_I = 1 s, sampleTime = 0.1 s.
+ */
+ PI1();
+ /** PI controller gain, integral time constant in seconds and sample time in seconds. */
+ PI1(double K_C, double T_I, double sampleTime);
+ /** Update PI controller gain, integral time constant and sample time. */
+ void setParameters(double K_C, double T_I, double sampleTime);
+ /** Set the controller lower and upper output limit. */
+ void setOutputLimits(double lowerOutputLimit, double upperOutputLimit);
+ /** Set the operation mode to "automatic" or "manual". */
+ void setOperationMode (Mode operationMode);
+ /** Set the controller output manually. */
+ void setOutputManually(double u_C);
+ /** Calculate the controller output u_C. */
+ double out();
+ /** Set the PI controller input e = SP - PV (setpoint - process value). */
+ void in(double e);
+ private:
+ double K_R, T_I, T_d, y, y_p1, u, u_p1, y_max, y_min, y_manual;
+ Mode operationMode;
+};
+
+#endif // PI1_H
\ No newline at end of file
diff -r 35738c77d454 -r 9e51de1050a1 AutomationElement/PT1.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AutomationElement/PT1.cpp Sat Nov 25 03:36:58 2017 +0000
@@ -0,0 +1,33 @@
+#include "PT1.h"
+
+PT1::PT1() {
+ u = y = u_p1 = y_p1 = 0;
+ setParameters(1, 1, 0.1);
+}
+
+PT1::PT1(double K_, double T_1_, double T_d_) {
+ u = y = u_p1 = y_p1 = 0;
+ setParameters(K_, T_1_, T_d_);
+}
+
+void PT1::setParameters(double K_, double T_1_, double T_d_) {
+ if (T_d_ > 0)
+ T_d = T_d_;
+ else
+ T_d = 0.1;
+ K = K_;
+ T_1 = T_1_;
+ a_1 = -exp(-T_d/T_1);
+ b_1 = K * (1 + a_1);
+}
+
+double PT1::out() {
+ y = -a_1 * y_p1 + b_1 * u_p1;
+ y_p1 = y;
+ u_p1 = u;
+ return y;
+}
+
+void PT1::in(double u_) {
+ u = u_;
+}
\ No newline at end of file
diff -r 35738c77d454 -r 9e51de1050a1 AutomationElement/PT1.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AutomationElement/PT1.h Sat Nov 25 03:36:58 2017 +0000
@@ -0,0 +1,30 @@
+#ifndef PT1_H
+#define PT1_H
+
+#include "mbed.h"
+
+/** Transfer function of a PT1 form.
+ * G(s) = Y(s) / U(s) = K / (1 + T_1 s)
+ *
+ * Author(s): TVZ Mechatronics Team
+ *
+ */
+class PT1 {
+ public:
+ /** Default constructor.
+ * K = 1, T_1 = 1, sampleTime = 0.1
+ */
+ PT1();
+ /** PT1 transfer function parameters and sample time in seconds. */
+ PT1(double K, double T_1, double sampleTime);
+ /** Update PT1 transfer function parameters and sample time. */
+ void setParameters(double K, double T_1, double sampleTime);
+ /** Calculate the output y. */
+ double out();
+ /** Set the input u. */
+ void in(double u);
+ private:
+ double K, T_1, T_d, a_1, b_1, y, y_p1, u, u_p1;
+};
+
+#endif // PT1_H
\ No newline at end of file
diff -r 35738c77d454 -r 9e51de1050a1 AutomationElement/PT2.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AutomationElement/PT2.cpp Sat Nov 25 03:36:58 2017 +0000
@@ -0,0 +1,42 @@
+#include "PT2.h"
+
+PT2::PT2() {
+ u = y = u_p1 = y_p1 = u_p2 = y_p2 = 0;
+ setParameters(1, 1, 0.5, 0.1);
+}
+
+PT2::PT2(double K_, double T_1_, double T_2_, double T_d_) {
+ u = y = u_p1 = y_p1 = u_p2 = y_p2 = 0;
+ setParameters(K_, T_1_, T_2_, T_d_);
+}
+
+void PT2::setParameters(double K_, double T_1_, double T_2_, double T_d_) {
+ if (T_d_ > 0)
+ T_d = T_d_;
+ else
+ T_d = 0.1;
+ K = K_;
+ T_1 = T_1_;
+ T_2 = T_2_;
+ a = 1 / T_1;
+ b = 1 / T_2;
+ c = exp(-a * T_d);
+ d = exp(-b * T_d);
+ b_1 = -K * ( (a - b)*(c + d) + b*(1 + d) - a*(1 + c) ) / (a - b);
+ b_2 = K * ( (a - b)*c*d + b*d - a*c ) / (a - b);
+ a_1 = -(c + d);
+ a_2 = c*d;
+}
+
+double PT2::out() {
+ y = -a_1 * y_p1 - a_2 * y_p2 + b_1 * u_p1 + b_2 * u_p2;
+ y_p2 = y_p1;
+ y_p1 = y;
+ u_p2 = u_p1;
+ u_p1 = u;
+ return y;
+}
+
+void PT2::in(double u_) {
+ u = u_;
+}
\ No newline at end of file
diff -r 35738c77d454 -r 9e51de1050a1 AutomationElement/PT2.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AutomationElement/PT2.h Sat Nov 25 03:36:58 2017 +0000
@@ -0,0 +1,30 @@
+#ifndef PT2_H
+#define PT2_H
+
+#include "mbed.h"
+
+/** Transfer function of a PT2 form.
+ * G(s) = Y(s) / U(s) = K / ( (1 + T_1 s) * (1 + T_2 s) )
+ *
+ * Author(s): TVZ Mechatronics Team
+ *
+ */
+class PT2 {
+ public:
+ /** Default constructor.
+ * K = 1, T_1 = 1 s, T_2 = 0.5 s, sampleTime = 0.1 s.
+ */
+ PT2();
+ /** PT2 transfer function parameters and sample time in seconds. */
+ PT2(double K, double T_1, double T_2, double sampleTime);
+ /** Update PT2 transfer function parameters and sample time. */
+ void setParameters(double K, double T_1, double T_2, double sampleTime);
+ /** Calculate the output y. */
+ double out();
+ /** Set the input u. */
+ void in(double u);
+ private:
+ double K, T_1, T_2, T_d, a, b, c, d, a_1, a_2, b_1, b_2, y, y_p1, y_p2, u, u_p1, u_p2;
+};
+
+#endif // PT2_H
\ No newline at end of file
diff -r 35738c77d454 -r 9e51de1050a1 AutomationElement/PT2cc.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AutomationElement/PT2cc.cpp Sat Nov 25 03:36:58 2017 +0000
@@ -0,0 +1,43 @@
+#include "PT2cc.h"
+
+PT2cc::PT2cc() {
+ u = y = u_p1 = y_p1 = u_p2 = y_p2 = 0;
+ setParameters(1, 0.707, 1, 0.1);
+}
+
+PT2cc::PT2cc(double K_, double zeta_, double w_n_, double T_d_) {
+ u = y = u_p1 = y_p1 = u_p2 = y_p2 = 0;
+ setParameters(K_, zeta_, w_n_, T_d_);
+}
+
+void PT2cc::setParameters(double K_, double zeta_, double w_n_, double T_d_) {
+ if (T_d_ > 0)
+ T_d = T_d_;
+ else
+ T_d = 0.1;
+ K = K_;
+ zeta = zeta_;
+ w_n = w_n_;
+ a = acos(zeta);
+ b = sqrt(1 - zeta*zeta);
+ c = exp(-zeta * w_n * T_d);
+ d = sin(w_n * b * T_d - a);
+ e = cos(w_n * b * T_d);
+ b_1 = K * (1 - 2*c*e - c*d/b);
+ b_2 = K * (c*c + c*d/b);
+ a_1 = -2*c*e;
+ a_2 = c*c;
+}
+
+double PT2cc::out() {
+ y = -a_1 * y_p1 - a_2 * y_p2 + b_1 * u_p1 + b_2 * u_p2;
+ y_p2 = y_p1;
+ y_p1 = y;
+ u_p2 = u_p1;
+ u_p1 = u;
+ return y;
+}
+
+void PT2cc::in(double u_) {
+ u = u_;
+}
\ No newline at end of file
diff -r 35738c77d454 -r 9e51de1050a1 AutomationElement/PT2cc.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AutomationElement/PT2cc.h Sat Nov 25 03:36:58 2017 +0000
@@ -0,0 +1,30 @@
+#ifndef PT2cc_H
+#define PT2cc_H
+
+#include "mbed.h"
+
+/** Transfer function of a general PT2 form. cc stands for conjugate-complex.
+ * G(s) = Y(s) / U(s) = K w_n^2 / ( s^2 + 2 zeta w_n + w_n^2 )
+ *
+ * Author(s): TVZ Mechatronics Team
+ *
+ */
+class PT2cc {
+ public:
+ /** Default constructor.
+ * K = 1, zeta = 0.707, w_n = 1 rad/s, sampleTime = 0.1 s.
+ */
+ PT2cc();
+ /** PT2cc transfer function parameters K, zeta, w_n and sample time in seconds. */
+ PT2cc(double K, double zeta, double w_n, double sampleTime);
+ /** Update PT2cc transfer function parameters and sample time. */
+ void setParameters(double K, double zeta, double w_n, double sampleTime);
+ /** Calculate the output y. */
+ double out();
+ /** Set the input u. */
+ void in(double u);
+ private:
+ double K, zeta, w_n, T_d, a, b, c, d, e, a_1, a_2, b_1, b_2, y, y_p1, y_p2, u, u_p1, u_p2;
+};
+
+#endif // PT2cc_H
\ No newline at end of file
diff -r 35738c77d454 -r 9e51de1050a1 AutomationElements.lib --- a/AutomationElements.lib Sat Nov 25 02:04:08 2017 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://os.mbed.com/users/tgw/code/AutomationElements/#a45cbb512c99
diff -r 35738c77d454 -r 9e51de1050a1 ESP8266.lib --- a/ESP8266.lib Sat Nov 25 02:04:08 2017 +0000 +++ b/ESP8266.lib Sat Nov 25 03:36:58 2017 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/teams/NetworkSocketAPI/code/ESP8266/#eb00dd185c7f +https://os.mbed.com/users/tgw/code/ESP8266/#d288181f5944
diff -r 35738c77d454 -r 9e51de1050a1 HALLFX/HALLFX_ENCODER.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/HALLFX/HALLFX_ENCODER.cpp Sat Nov 25 03:36:58 2017 +0000
@@ -0,0 +1,20 @@
+#include "HALLFX_ENCODER.h"
+
+HALLFX_ENCODER::HALLFX_ENCODER(PinName enc_in): _enc_in(enc_in){
+ _enc_in.mode(PullUp);
+ // Invoke interrupt on both falling and rising edges
+ _enc_in.fall(callback(this, &HALLFX_ENCODER::callback_c));
+ _enc_in.rise(callback(this, &HALLFX_ENCODER::callback_c));
+}
+
+long HALLFX_ENCODER::read(){
+ return count;
+}
+
+void HALLFX_ENCODER::reset(){
+ count = 0;
+}
+
+void HALLFX_ENCODER::callback_c(){
+ count++;
+}
\ No newline at end of file
diff -r 35738c77d454 -r 9e51de1050a1 HALLFX/HALLFX_ENCODER.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/HALLFX/HALLFX_ENCODER.h Sat Nov 25 03:36:58 2017 +0000
@@ -0,0 +1,38 @@
+#ifndef HALLFX_ENCODER_H
+#define HALLFX_ENCODER_H
+
+/*
+ Basic Encoder Library for Sparkfun's Wheel Encoder Kit
+ Part# ROB-12629.
+*/
+
+#include "mbed.h"
+
+class HALLFX_ENCODER{
+ public:
+ /*
+ Constructor for Encoder objects
+ @param enc_in The mBed pin connected to encoder output
+ */
+ HALLFX_ENCODER(PinName enc_in);
+ /*
+ read() returns total number of counts of the encoder.
+ Count can be +/- and indicates the overall direction,
+ (+): CW (-): CCW
+ @return The toltal number of counts of the encoder.
+ */
+ long read();
+ /*
+ reset() clears the counter to 0.
+ */
+ void reset();
+ private:
+ long count; // Total number of counts since start.
+ InterruptIn _enc_in;// Encoder Input/Interrupt Pin
+ /*
+ Increments/Decrements count on interrrupt.
+ */
+ void callback_c(); // Interrupt callback function
+};
+
+#endif
\ No newline at end of file
diff -r 35738c77d454 -r 9e51de1050a1 HALLFX_ENCODER.lib --- a/HALLFX_ENCODER.lib Sat Nov 25 02:04:08 2017 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://developer.mbed.org/users/electromotivated/code/HALLFX_ENCODER/#f10558519825
diff -r 35738c77d454 -r 9e51de1050a1 MotorDriver.lib --- a/MotorDriver.lib Sat Nov 25 02:04:08 2017 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://os.mbed.com/users/tgw/code/MotorDriver/#19476b49822b
diff -r 35738c77d454 -r 9e51de1050a1 MotorDriver/MotorDriver.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorDriver/MotorDriver.cpp Sat Nov 25 03:36:58 2017 +0000
@@ -0,0 +1,96 @@
+#include "MotorDriver.h"
+#include "mbed.h"
+#include <cmath>
+
+// See .h file for function descriptions and details
+
+MotorDriver::MotorDriver(DigitalOut in1, DigitalOut in2, PwmOut pwm, float pwmFreq, bool isBrakeable)
+ : _in1(in1), _in2(in2), _pwm(pwm)
+{
+ // Initialize motor and variables
+ _in1 = 1; _in2 = 0; _pwm = 0.0; this->isBrakeable = isBrakeable;
+ _pwm.period(1.0/pwmFreq);
+ determineState();
+}
+
+void MotorDriver::determineState(){
+ if(_in1==1 && _in2==1){
+ motor_state.code = BRAKING;
+ motor_state.value = _pwm;
+ }
+ else if(_in1==1 && _in2==0){
+ motor_state.code = DRIVING_CW;
+ motor_state.value = _pwm;
+ }
+ else if(_in1==0 && _in2==1){
+ motor_state.code = DRIVING_CCW;
+ motor_state.value = _pwm;
+ }
+ else if(_in1==0 && _in2==0){
+ motor_state.code = COASTING;
+ motor_state.value = _pwm;
+ }
+ else{
+ motor_state.code = ERROR_M; // Undefined config found
+ motor_state.value = _pwm;
+ }
+}
+
+State_t MotorDriver::setSpeed(float speed){
+ // Prevent instantaneous reversal; put into brake if requested to do so
+ bool isTryingToInstantReverse =
+ (bool)((speed < 0.0) && (motor_state.code == DRIVING_CW) && (_pwm > 0.05)) ||
+ (bool)((speed > 0.0) && (motor_state.code == DRIVING_CCW)&& (_pwm > 0.05));
+ if(isTryingToInstantReverse){
+ // Set motor to brake, set state to error
+ coast();
+ motor_state.code = ERROR_M; motor_state.value = _pwm;
+ return motor_state;
+ }
+ else{
+ if(speed == 0.0)
+ {
+ // No effect to _in pins
+ }
+ else{
+ _in1 = (speed>0.0);
+ _in2 = (speed<0.0);
+ }
+ _pwm = std::abs(speed);
+ determineState();
+ return motor_state;
+ }
+}
+
+State_t MotorDriver::forceSetSpeed(float speed){
+ if(speed == 0.0)
+ {
+ // No effect to _in pins
+ }
+ else{
+ _in1 = (speed>0.0);
+ _in2 = (speed<0.0);
+ }
+ _pwm = std::abs(speed);
+ determineState();
+ return motor_state;
+}
+
+State_t MotorDriver::brake(float intensity){
+ if(!isBrakeable) coast();
+ else{
+ _in1=1; _in2=1; _pwm = _pwm * (1.0-intensity);
+ }
+ determineState();
+ return motor_state;
+}
+
+State_t MotorDriver::coast(){
+ _in1=0; _in2=0; _pwm = 1.0;
+ determineState();
+ return motor_state;
+}
+
+State_t MotorDriver::getState(){
+ return motor_state;
+}
\ No newline at end of file
diff -r 35738c77d454 -r 9e51de1050a1 MotorDriver/MotorDriver.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorDriver/MotorDriver.h Sat Nov 25 03:36:58 2017 +0000
@@ -0,0 +1,103 @@
+/*
+Bryce Williams
+09/14/2014
+
+General/Basic Motor Driver Class providing Motor Control using a COTS Motor Driver
+
+ Class based off of Christopher Hasler's Motordriver library found at
+ https://developer.mbed.org/cookbook/Motor
+
+
+* Permission is hereby granted, free of charge, to any person obtaining a copy
+* of this software and associated documentation files (the "Software"), to deal
+* in the Software without restriction, including without limitation the rights
+* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+* copies of the Software, and to permit persons to whom the Software is
+* furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in
+* all copies or substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+* THE SOFTWARE.
+*/
+
+#ifndef MOTORDRIVER_H
+#define MOTORDRIVER_H
+
+#include "mbed.h"
+
+typedef enum {ERROR_M=0, DRIVING_CW=1, DRIVING_CCW=2, BRAKING=3, COASTING=4}Code_t;
+typedef struct{
+ Code_t code;
+ float value;
+}State_t;
+
+class MotorDriver{
+ public:
+ /*
+ Constructor of MotorDriver Objects
+ @param in_1 Direction Input 1
+ @param in_2 Direction Input 2
+ @param pwm PWM speed control input
+ @param pwmFreq PWM frequency, some motors may whine at lower freqs
+ @param isBrakable Boolean value indicating whether or not the
+ motor driver is brakeable(see your datasheet)
+ */
+ MotorDriver(DigitalOut in1, DigitalOut in2, PwmOut pwm, float pwmFreq, bool isBrakeable = false);
+
+ /*
+ Sets speed of motor normalized between -1.0 to 1.0
+ @param speed Value -1.0 to 1.0 (>0 CW at speed as percentage)
+ (<0 CCW at speed as percentage)
+ (=0 speed is zero)
+ @return state of the motor
+ NOTE: This method will NOT allow user to instantaneously swithch
+ from CW to CCW or vise versa. Doing so will cause the motor to
+ be put in to a BRAKE condition, while the motor_state.code will
+ be updated to ERROR. User should avoid trying to do this, call
+ first setSpeed(0) or brake().
+ */
+ State_t setSpeed(float speed);
+
+ /*
+ Same as setSpeed(float speed), however does not impose the safety disallowing
+ instantaneous reversal of motor direction. It is up to the user to ensure they
+ do not blow up their motor.
+ */
+ State_t forceSetSpeed(float speed);
+
+ /*
+ Put motor into braked config
+ @param intensity How hard to brake (0.0 to 1.0)
+ @return state of the motors
+ */
+ State_t brake(float intensity);
+
+ /*
+ Put motor into stop/coast config
+ */
+ State_t coast();
+
+
+ /*
+ Get state of the motor
+ @return state of the motor
+ */
+ State_t getState();
+
+ protected: // Protected so objects that inherit can access
+ State_t motor_state;
+ DigitalOut _in1;
+ DigitalOut _in2;
+ PwmOut _pwm;
+ bool isBrakeable;
+ void determineState(); // Determine motor state based on in1, in2, and pwm
+};
+
+#endif
\ No newline at end of file
diff -r 35738c77d454 -r 9e51de1050a1 PID.lib --- a/PID.lib Sat Nov 25 02:04:08 2017 +0000 +++ b/PID.lib Sat Nov 25 03:36:58 2017 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/users/electromotivated/code/PID/#0cf2f6d13c71 +https://os.mbed.com/users/tgw/code/PID/#fc06979d29e0
diff -r 35738c77d454 -r 9e51de1050a1 main.cpp
--- a/main.cpp Sat Nov 25 02:04:08 2017 +0000
+++ b/main.cpp Sat Nov 25 03:36:58 2017 +0000
@@ -265,7 +265,7 @@
wait(1);
thread_motor.start(callback(motorThread));
wait(1);
- //thread_sonar.start(callback(sonarThread));
+ thread_sonar.start(callback(sonarThread));
//Thread thread_sonar(sonarThread);
}