Energy harvesting mobile robot. Developed at Institute of Systems and Robotics — University of Coimbra.

Dependents:   Mapping VirtualForces_debug OneFileToRuleThemAll VirtualForces_with_class ... more

Files at this revision

API Documentation at this revision

Comitter:
ISR
Date:
Thu Feb 02 12:21:11 2017 +0000
Commit message:
Initial commit.

Changed in this revision

VCNL40x0.cpp Show annotated file Show diff for this revision Revisions of this file
VCNL40x0.h Show annotated file Show diff for this revision Revisions of this file
nRF24L01P.cpp Show annotated file Show diff for this revision Revisions of this file
nRF24L01P.h Show annotated file Show diff for this revision Revisions of this file
robot.h Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VCNL40x0.cpp	Thu Feb 02 12:21:11 2017 +0000
@@ -0,0 +1,288 @@
+/*
+Copyright (c) 2012 Vishay GmbH, www.vishay.com
+author: DS, version 1.21
+
+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.
+*/
+
+#include "VCNL40x0.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+VCNL40x0::VCNL40x0(PinName sda, PinName scl, unsigned char addr) : _i2c(sda, scl), _addr(addr) {
+    _i2c.frequency(1000000);                                // set I2C frequency to 1MHz
+}
+
+VCNL40x0::~VCNL40x0() {
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+VCNL40x0Error_e VCNL40x0::SetCommandRegister (unsigned char Command) {
+
+    _send[0] = REGISTER_COMMAND;                            // VCNL40x0 Configuration reister
+    _send[1] = Command;
+    _i2c.write(VCNL40x0_ADDRESS,_send, 2);                  // Write 2 bytes on I2C
+
+    return VCNL40x0_ERROR_OK;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+VCNL40x0Error_e VCNL40x0::ReadCommandRegister (unsigned char *Command) {
+
+    _send[0] = REGISTER_COMMAND;                            // VCNL40x0 Configuration register
+    _i2c.write(VCNL40x0_ADDRESS,_send, 1);                  // Write 1 byte on I2C
+    _i2c.read(VCNL40x0_ADDRESS+1,_receive, 1);              // Read 1 byte on I2C
+
+    *Command = (unsigned char)(_receive[0]);
+
+    return VCNL40x0_ERROR_OK;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+VCNL40x0Error_e VCNL40x0::ReadID (unsigned char *ID) {
+
+    _send[0] = REGISTER_ID;                                 // VCNL40x0 product ID revision register
+    _i2c.write(VCNL40x0_ADDRESS, _send, 1);                 // Write 1 byte on I2C
+    _i2c.read(VCNL40x0_ADDRESS+1, _receive, 1);             // Read 1 byte on I2C
+
+    *ID = (unsigned char)(_receive[0]);
+
+    return VCNL40x0_ERROR_OK;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+VCNL40x0Error_e VCNL40x0::SetCurrent (unsigned char Current) {
+
+    _send[0] = REGISTER_PROX_CURRENT;                       // VCNL40x0 IR LED Current register
+    _send[1] = Current;
+    _i2c.write(VCNL40x0_ADDRESS,_send, 2);                  // Write 2 bytes on I2C
+
+    return VCNL40x0_ERROR_OK;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+VCNL40x0Error_e VCNL40x0::ReadCurrent (unsigned char *Current) {
+
+    _send[0] = REGISTER_PROX_CURRENT;                       // VCNL40x0 IR LED current register
+    _i2c.write(VCNL40x0_ADDRESS,_send, 1);                  // Write 1 byte on I2C
+    _i2c.read(VCNL40x0_ADDRESS+1,_receive, 1);              // Read 1 byte on I2C
+
+    *Current = (unsigned char)(_receive[0]);
+
+    return VCNL40x0_ERROR_OK;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+VCNL40x0Error_e VCNL40x0::SetProximityRate (unsigned char ProximityRate) {
+
+    _send[0] = REGISTER_PROX_RATE;                          // VCNL40x0 Proximity rate register
+    _send[1] = ProximityRate;
+    _i2c.write(VCNL40x0_ADDRESS,_send, 2);                  // Write 2 bytes on I2C
+
+    return VCNL40x0_ERROR_OK;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+VCNL40x0Error_e VCNL40x0::SetAmbiConfiguration (unsigned char AmbiConfiguration) {
+
+    _send[0] = REGISTER_AMBI_PARAMETER;                     // VCNL40x0 Ambilight configuration
+    _send[1] = AmbiConfiguration;
+    _i2c.write(VCNL40x0_ADDRESS,_send, 2);                  // Write 2 bytes on I2C
+
+    return VCNL40x0_ERROR_OK;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+VCNL40x0Error_e VCNL40x0::SetInterruptControl (unsigned char InterruptControl) {
+
+    _send[0] = REGISTER_INTERRUPT_CONTROL;                  // VCNL40x0 Interrupt Control register
+    _send[1] = InterruptControl;
+    _i2c.write(VCNL40x0_ADDRESS,_send, 2);                  // Write 2 bytes on I2C
+
+    return VCNL40x0_ERROR_OK;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+VCNL40x0Error_e VCNL40x0::ReadInterruptControl (unsigned char *InterruptControl) {
+
+    _send[0] = REGISTER_INTERRUPT_CONTROL;                  // VCNL40x0 Interrupt Control register
+    _i2c.write(VCNL40x0_ADDRESS,_send, 1);                  // Write 1 byte on I2C
+    _i2c.read(VCNL40x0_ADDRESS+1,_receive, 1);              // Read 1 byte on I2C
+
+    *InterruptControl = (unsigned char)(_receive[0]);
+
+    return VCNL40x0_ERROR_OK;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+VCNL40x0Error_e VCNL40x0::SetInterruptStatus (unsigned char InterruptStatus) {
+
+    _send[0] = REGISTER_INTERRUPT_STATUS;                   // VCNL40x0 Interrupt Status register
+    _send[1] = InterruptStatus;
+    _i2c.write(VCNL40x0_ADDRESS,_send, 2);                  // Write 2 bytes on I2C
+
+    return VCNL40x0_ERROR_OK;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+VCNL40x0Error_e VCNL40x0::SetModulatorTimingAdjustment (unsigned char ModulatorTimingAdjustment) {
+
+    _send[0] = REGISTER_PROX_TIMING;                        // VCNL40x0 Modulator Timing Adjustment register
+    _send[1] = ModulatorTimingAdjustment;
+    _i2c.write(VCNL40x0_ADDRESS,_send, 2);                  // Write 2 bytes on I2C
+
+    return VCNL40x0_ERROR_OK;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+VCNL40x0Error_e VCNL40x0::ReadInterruptStatus (unsigned char *InterruptStatus) {
+
+    _send[0] = REGISTER_INTERRUPT_STATUS;                   // VCNL40x0 Interrupt Status register
+    _i2c.write(VCNL40x0_ADDRESS,_send, 1);                  // Write 1 byte on I2C
+    _i2c.read(VCNL40x0_ADDRESS+1,_receive, 1);              // Read 1 byte on I2C
+
+    *InterruptStatus = (unsigned char)(_receive[0]);
+
+    return VCNL40x0_ERROR_OK;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+VCNL40x0Error_e VCNL40x0::ReadProxiValue (unsigned int *ProxiValue) {
+
+    _send[0] = REGISTER_PROX_VALUE;                         // VCNL40x0 Proximity Value register
+    _i2c.write(VCNL40x0_ADDRESS, _send, 1);                 // Write 1 byte on I2C
+    _i2c.read(VCNL40x0_ADDRESS+1, _receive, 2);             // Read 2 bytes on I2C
+    *ProxiValue = ((unsigned int)_receive[0] << 8 | (unsigned char)_receive[1]);
+
+    return VCNL40x0_ERROR_OK;
+
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+VCNL40x0Error_e VCNL40x0::ReadAmbiValue (unsigned int *AmbiValue) {
+
+    _send[0] = REGISTER_AMBI_VALUE;                          // VCNL40x0 Ambient Light Value register
+    _i2c.write(VCNL40x0_ADDRESS, _send, 1);                  // Write 1 byte on I2C
+    _i2c.read(VCNL40x0_ADDRESS+1, _receive, 2);              // Read 2 bytes on I2C
+    *AmbiValue = ((unsigned int)_receive[0] << 8 | (unsigned char)_receive[1]);
+
+    return VCNL40x0_ERROR_OK;
+
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+VCNL40x0Error_e VCNL40x0::SetLowThreshold (unsigned int LowThreshold) {
+
+    unsigned char LoByte=0, HiByte=0;
+    
+    LoByte = (unsigned char)(LowThreshold & 0x00ff);
+    HiByte = (unsigned char)((LowThreshold & 0xff00)>>8);
+    
+    _send[0] = REGISTER_INTERRUPT_LOW_THRES;                // VCNL40x0 Low Threshold Register, Hi Byte
+    _send[1] = HiByte;
+    _i2c.write(VCNL40x0_ADDRESS,_send, 2);                  // Write 2 bytes on I2C
+    
+    _send[0] = REGISTER_INTERRUPT_LOW_THRES+1;              // VCNL40x0 Low Threshold Register, Lo Byte
+    _send[1] = LoByte;
+    _i2c.write(VCNL40x0_ADDRESS,_send, 2);                  // Write 2 bytes on I2C
+
+    return VCNL40x0_ERROR_OK;
+
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+VCNL40x0Error_e VCNL40x0::SetHighThreshold (unsigned int HighThreshold) {
+
+    unsigned char LoByte=0, HiByte=0;
+    
+    LoByte = (unsigned char)(HighThreshold & 0x00ff);
+    HiByte = (unsigned char)((HighThreshold & 0xff00)>>8);
+    
+    _send[0] = REGISTER_INTERRUPT_HIGH_THRES;               // VCNL40x0 High Threshold Register, Hi Byte
+    _send[1] = HiByte;
+    _i2c.write(VCNL40x0_ADDRESS,_send, 2);                  // Write 2 bytes on I2C
+    
+    _send[0] = REGISTER_INTERRUPT_HIGH_THRES+1;             // VCNL40x0 High Threshold Register, Lo Byte
+    _send[1] = LoByte;
+    _i2c.write(VCNL40x0_ADDRESS,_send, 2);                  // Write 2 bytes on I2C
+
+    return VCNL40x0_ERROR_OK;
+
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+VCNL40x0Error_e VCNL40x0::ReadProxiOnDemand (unsigned int *ProxiValue) {
+
+    unsigned char Command=0;
+
+    // enable prox value on demand
+    SetCommandRegister (COMMAND_PROX_ENABLE | COMMAND_PROX_ON_DEMAND);
+ 
+    // wait on prox data ready bit
+    do {
+        ReadCommandRegister (&Command);                     // read command register
+    } while (!(Command & COMMAND_MASK_PROX_DATA_READY));
+
+    ReadProxiValue (ProxiValue);                            // read prox value
+
+    SetCommandRegister (COMMAND_ALL_DISABLE);               // stop prox value on demand
+    
+    return VCNL40x0_ERROR_OK;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+VCNL40x0Error_e VCNL40x0::ReadAmbiOnDemand (unsigned int *AmbiValue) {
+
+    unsigned char Command=0;
+
+    // enable ambi value on demand
+    SetCommandRegister (COMMAND_PROX_ENABLE | COMMAND_AMBI_ON_DEMAND);
+
+    // wait on ambi data ready bit
+    do {
+        ReadCommandRegister (&Command);                     // read command register
+    } while (!(Command & COMMAND_MASK_AMBI_DATA_READY));
+
+    ReadAmbiValue (AmbiValue);                              // read ambi value    
+
+    SetCommandRegister (COMMAND_ALL_DISABLE);               // stop ambi value on demand    
+
+    return VCNL40x0_ERROR_OK;
+}
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VCNL40x0.h	Thu Feb 02 12:21:11 2017 +0000
@@ -0,0 +1,190 @@
+/*
+Copyright (c) 2012 Vishay GmbH, www.vishay.com
+author: DS, version 1.2
+
+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 VCNL40x0_H
+#define VCNL40x0_H
+
+#include "mbed.h"
+
+// Library for the Vishay Proximity/Ambient Light Sensor VCNL4010/4020
+// The VCNL4x00 is a I2C digital Proximity and Ambient Light Sensor in a small SMD package
+
+#define VCNL40x0_ADDRESS               (0x26) // 001 0011 shifted left 1 bit = 0x26
+
+// registers 
+#define REGISTER_COMMAND               (0x80)
+#define REGISTER_ID                    (0x81)
+#define REGISTER_PROX_RATE             (0x82)
+#define REGISTER_PROX_CURRENT          (0x83)
+#define REGISTER_AMBI_PARAMETER        (0x84)
+#define REGISTER_AMBI_VALUE            (0x85)
+#define REGISTER_PROX_VALUE            (0x87)
+#define REGISTER_INTERRUPT_CONTROL     (0x89)
+#define REGISTER_INTERRUPT_LOW_THRES   (0x8a)
+#define REGISTER_INTERRUPT_HIGH_THRES  (0x8c)
+#define REGISTER_INTERRUPT_STATUS      (0x8e)
+#define REGISTER_PROX_TIMING           (0x8f)
+#define REGISTER_AMBI_IR_LIGHT_LEVEL   (0x90)   // This register is not intended to be use by customer
+
+// Bits in Command register (0x80)
+#define COMMAND_ALL_DISABLE            (0x00)
+#define COMMAND_SELFTIMED_MODE_ENABLE  (0x01)
+#define COMMAND_PROX_ENABLE            (0x02)
+#define COMMAND_AMBI_ENABLE            (0x04)
+#define COMMAND_PROX_ON_DEMAND         (0x08)
+#define COMMAND_AMBI_ON_DEMAND         (0x10)
+#define COMMAND_MASK_PROX_DATA_READY   (0x20)
+#define COMMAND_MASK_AMBI_DATA_READY   (0x40)
+#define COMMAND_MASK_LOCK              (0x80)
+
+// Bits in Product ID Revision Register (0x81)
+#define PRODUCT_MASK_REVISION_ID       (0x0f)
+#define PRODUCT_MASK_PRODUCT_ID        (0xf0)
+
+// Bits in Prox Measurement Rate register (0x82)
+#define PROX_MEASUREMENT_RATE_2        (0x00)   // DEFAULT
+#define PROX_MEASUREMENT_RATE_4        (0x01)
+#define PROX_MEASUREMENT_RATE_8        (0x02)
+#define PROX_MEASUREMENT_RATE_16       (0x03)
+#define PROX_MEASUREMENT_RATE_31       (0x04)
+#define PROX_MEASUREMENT_RATE_62       (0x05)
+#define PROX_MEASUREMENT_RATE_125      (0x06)
+#define PROX_MEASUREMENT_RATE_250      (0x07)
+#define PROX_MASK_MEASUREMENT_RATE     (0x07)
+
+// Bits in Procimity LED current setting (0x83)
+#define PROX_MASK_LED_CURRENT          (0x3f)   // DEFAULT = 2
+#define PROX_MASK_FUSE_PROG_ID         (0xc0)
+
+// Bits in Ambient Light Parameter register (0x84)
+#define AMBI_PARA_AVERAGE_1            (0x00)
+#define AMBI_PARA_AVERAGE_2            (0x01)
+#define AMBI_PARA_AVERAGE_4            (0x02)
+#define AMBI_PARA_AVERAGE_8            (0x03)
+#define AMBI_PARA_AVERAGE_16           (0x04)
+#define AMBI_PARA_AVERAGE_32           (0x05)   // DEFAULT
+#define AMBI_PARA_AVERAGE_64           (0x06)
+#define AMBI_PARA_AVERAGE_128          (0x07)
+#define AMBI_MASK_PARA_AVERAGE         (0x07)
+
+#define AMBI_PARA_AUTO_OFFSET_ENABLE   (0x08)   // DEFAULT enable
+#define AMBI_MASK_PARA_AUTO_OFFSET     (0x08)
+
+#define AMBI_PARA_MEAS_RATE_1          (0x00)
+#define AMBI_PARA_MEAS_RATE_2          (0x10)   // DEFAULT
+#define AMBI_PARA_MEAS_RATE_3          (0x20)
+#define AMBI_PARA_MEAS_RATE_4          (0x30)
+#define AMBI_PARA_MEAS_RATE_5          (0x40)
+#define AMBI_PARA_MEAS_RATE_6          (0x50)
+#define AMBI_PARA_MEAS_RATE_8          (0x60)
+#define AMBI_PARA_MEAS_RATE_10         (0x70)
+#define AMBI_MASK_PARA_MEAS_RATE       (0x70)
+
+#define AMBI_PARA_CONT_CONV_ENABLE     (0x80)
+#define AMBI_MASK_PARA_CONT_CONV       (0x80)   // DEFAULT disable
+
+// Bits in Interrupt Control Register (x89)
+#define INTERRUPT_THRES_SEL_PROX       (0x00)
+#define INTERRUPT_THRES_SEL_ALS        (0x01)
+
+#define INTERRUPT_THRES_ENABLE         (0x02)
+
+#define INTERRUPT_ALS_READY_ENABLE     (0x04)
+
+#define INTERRUPT_PROX_READY_ENABLE    (0x08)
+
+#define INTERRUPT_COUNT_EXCEED_1       (0x00)   // DEFAULT
+#define INTERRUPT_COUNT_EXCEED_2       (0x20)
+#define INTERRUPT_COUNT_EXCEED_4       (0x40)
+#define INTERRUPT_COUNT_EXCEED_8       (0x60)
+#define INTERRUPT_COUNT_EXCEED_16      (0x80)
+#define INTERRUPT_COUNT_EXCEED_32      (0xa0)
+#define INTERRUPT_COUNT_EXCEED_64      (0xc0)
+#define INTERRUPT_COUNT_EXCEED_128     (0xe0)
+#define INTERRUPT_MASK_COUNT_EXCEED    (0xe0) 
+
+// Bits in Interrupt Status Register (x8e)
+#define INTERRUPT_STATUS_THRES_HI      (0x01)
+#define INTERRUPT_STATUS_THRES_LO      (0x02)
+#define INTERRUPT_STATUS_ALS_READY     (0x04)
+#define INTERRUPT_STATUS_PROX_READY    (0x08)
+#define INTERRUPT_MASK_STATUS_THRES_HI (0x01)
+#define INTERRUPT_MASK_THRES_LO        (0x02)
+#define INTERRUPT_MASK_ALS_READY       (0x04)
+#define INTERRUPT_MASK_PROX_READY      (0x08)
+
+typedef enum {
+    VCNL40x0_ERROR_OK = 0,               // Everything executed normally
+    VCNL40x0_ERROR_I2CINIT,              // Unable to initialise I2C
+    VCNL40x0_ERROR_I2CBUSY,              // I2C already in use
+    VCNL40x0_ERROR_LAST
+} VCNL40x0Error_e;
+
+
+class VCNL40x0 {
+public:
+// Creates an instance of the class.
+// Connect module at I2C address addr using I2C port pins sda and scl.
+
+    VCNL40x0 (PinName sda, PinName scl, unsigned char addr);
+
+// Destroys instance
+
+    ~VCNL40x0();
+
+// public functions
+
+    VCNL40x0Error_e Init (void);
+    
+    VCNL40x0Error_e SetCommandRegister (unsigned char Command);
+    VCNL40x0Error_e SetCurrent (unsigned char CurrentValue);
+    VCNL40x0Error_e SetProximityRate (unsigned char ProximityRate);
+    VCNL40x0Error_e SetAmbiConfiguration (unsigned char AmbiConfiguration);
+    VCNL40x0Error_e SetLowThreshold (unsigned int LowThreshold);
+    VCNL40x0Error_e SetHighThreshold (unsigned int HighThreshold);
+    VCNL40x0Error_e SetInterruptControl (unsigned char InterruptControl);
+    VCNL40x0Error_e SetInterruptStatus (unsigned char InterruptStatus);
+    VCNL40x0Error_e SetModulatorTimingAdjustment (unsigned char ModulatorTimingAdjustment);
+
+    VCNL40x0Error_e ReadID (unsigned char *ID);
+    VCNL40x0Error_e ReadCurrent (unsigned char *CurrentValue);
+    VCNL40x0Error_e ReadCommandRegister (unsigned char *Command);
+    VCNL40x0Error_e ReadProxiValue (unsigned int *ProxiValue);
+    VCNL40x0Error_e ReadAmbiValue (unsigned int *AmbiValue);
+    VCNL40x0Error_e ReadInterruptStatus (unsigned char *InterruptStatus);
+    VCNL40x0Error_e ReadInterruptControl (unsigned char *InterruptControl);
+    
+    VCNL40x0Error_e ReadProxiOnDemand (unsigned int *ProxiValue);
+    VCNL40x0Error_e ReadAmbiOnDemand (unsigned int *AmbiValue);
+ 
+private:
+    I2C _i2c;
+    int _addr;
+    char _send[3];
+    char _receive[2];
+    
+};
+
+#endif
+
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/nRF24L01P.cpp	Thu Feb 02 12:21:11 2017 +0000
@@ -0,0 +1,1030 @@
+/**
+ * @file nRF24L01P.cpp
+ *
+ * @author Owen Edwards
+ * 
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 Owen Edwards
+ *
+ *    This program is free software: you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation, either version 3 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License
+ *    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * @section DESCRIPTION
+ *
+ * nRF24L01+ Single Chip 2.4GHz Transceiver from Nordic Semiconductor.
+ *
+ * Datasheet:
+ *
+ * http://www.nordicsemi.no/files/Product/data_sheet/nRF24L01P_Product_Specification_1_0.pdf
+ */
+
+/**
+ * Includes
+ */
+#include "nRF24L01P.h"
+
+/**
+ * Defines
+ *
+ * (Note that all defines here start with an underscore, e.g. '_NRF24L01P_MODE_UNKNOWN',
+ *  and are local to this library.  The defines in the nRF24L01P.h file do not start
+ *  with the underscore, and can be used by code to access this library.)
+ */
+
+typedef enum {
+    _NRF24L01P_MODE_UNKNOWN,
+    _NRF24L01P_MODE_POWER_DOWN,
+    _NRF24L01P_MODE_STANDBY,
+    _NRF24L01P_MODE_RX,
+    _NRF24L01P_MODE_TX,
+} nRF24L01P_Mode_Type;
+
+/*
+ * The following FIFOs are present in nRF24L01+:
+ *   TX three level, 32 byte FIFO
+ *   RX three level, 32 byte FIFO
+ */
+#define _NRF24L01P_TX_FIFO_COUNT   3
+#define _NRF24L01P_RX_FIFO_COUNT   3
+
+#define _NRF24L01P_TX_FIFO_SIZE   32
+#define _NRF24L01P_RX_FIFO_SIZE   32
+
+#define _NRF24L01P_SPI_MAX_DATA_RATE     10000000
+
+#define _NRF24L01P_SPI_CMD_RD_REG            0x00
+#define _NRF24L01P_SPI_CMD_WR_REG            0x20
+#define _NRF24L01P_SPI_CMD_RD_RX_PAYLOAD     0x61   
+#define _NRF24L01P_SPI_CMD_WR_TX_PAYLOAD     0xa0
+#define _NRF24L01P_SPI_CMD_FLUSH_TX          0xe1
+#define _NRF24L01P_SPI_CMD_FLUSH_RX          0xe2
+#define _NRF24L01P_SPI_CMD_REUSE_TX_PL       0xe3
+#define _NRF24L01P_SPI_CMD_R_RX_PL_WID       0x60
+#define _NRF24L01P_SPI_CMD_W_ACK_PAYLOAD     0xa8
+#define _NRF24L01P_SPI_CMD_W_TX_PYLD_NO_ACK  0xb0
+#define _NRF24L01P_SPI_CMD_NOP               0xff
+
+
+#define _NRF24L01P_REG_CONFIG                0x00
+#define _NRF24L01P_REG_EN_AA                 0x01
+#define _NRF24L01P_REG_EN_RXADDR             0x02
+#define _NRF24L01P_REG_SETUP_AW              0x03
+#define _NRF24L01P_REG_SETUP_RETR            0x04
+#define _NRF24L01P_REG_RF_CH                 0x05
+#define _NRF24L01P_REG_RF_SETUP              0x06
+#define _NRF24L01P_REG_STATUS                0x07
+#define _NRF24L01P_REG_OBSERVE_TX            0x08
+#define _NRF24L01P_REG_RPD                   0x09
+#define _NRF24L01P_REG_RX_ADDR_P0            0x0a
+#define _NRF24L01P_REG_RX_ADDR_P1            0x0b
+#define _NRF24L01P_REG_RX_ADDR_P2            0x0c
+#define _NRF24L01P_REG_RX_ADDR_P3            0x0d
+#define _NRF24L01P_REG_RX_ADDR_P4            0x0e
+#define _NRF24L01P_REG_RX_ADDR_P5            0x0f
+#define _NRF24L01P_REG_TX_ADDR               0x10
+#define _NRF24L01P_REG_RX_PW_P0              0x11
+#define _NRF24L01P_REG_RX_PW_P1              0x12
+#define _NRF24L01P_REG_RX_PW_P2              0x13
+#define _NRF24L01P_REG_RX_PW_P3              0x14
+#define _NRF24L01P_REG_RX_PW_P4              0x15
+#define _NRF24L01P_REG_RX_PW_P5              0x16
+#define _NRF24L01P_REG_FIFO_STATUS           0x17
+#define _NRF24L01P_REG_DYNPD                 0x1c
+#define _NRF24L01P_REG_FEATURE               0x1d
+
+#define _NRF24L01P_REG_ADDRESS_MASK          0x1f
+
+// CONFIG register:
+#define _NRF24L01P_CONFIG_PRIM_RX        (1<<0)
+#define _NRF24L01P_CONFIG_PWR_UP         (1<<1)
+#define _NRF24L01P_CONFIG_CRC0           (1<<2)
+#define _NRF24L01P_CONFIG_EN_CRC         (1<<3)
+#define _NRF24L01P_CONFIG_MASK_MAX_RT    (1<<4)
+#define _NRF24L01P_CONFIG_MASK_TX_DS     (1<<5)
+#define _NRF24L01P_CONFIG_MASK_RX_DR     (1<<6)
+
+#define _NRF24L01P_CONFIG_CRC_MASK       (_NRF24L01P_CONFIG_EN_CRC|_NRF24L01P_CONFIG_CRC0)
+#define _NRF24L01P_CONFIG_CRC_NONE       (0)
+#define _NRF24L01P_CONFIG_CRC_8BIT       (_NRF24L01P_CONFIG_EN_CRC)
+#define _NRF24L01P_CONFIG_CRC_16BIT      (_NRF24L01P_CONFIG_EN_CRC|_NRF24L01P_CONFIG_CRC0)
+
+// EN_AA register:
+#define _NRF24L01P_EN_AA_NONE            0
+
+// EN_RXADDR register:
+#define _NRF24L01P_EN_RXADDR_NONE        0
+
+// SETUP_AW register:
+#define _NRF24L01P_SETUP_AW_AW_MASK      (0x3<<0)
+#define _NRF24L01P_SETUP_AW_AW_3BYTE     (0x1<<0)
+#define _NRF24L01P_SETUP_AW_AW_4BYTE     (0x2<<0)
+#define _NRF24L01P_SETUP_AW_AW_5BYTE     (0x3<<0)
+
+// SETUP_RETR register:
+#define _NRF24L01P_SETUP_RETR_NONE       0
+
+// RF_SETUP register:
+#define _NRF24L01P_RF_SETUP_RF_PWR_MASK          (0x3<<1)
+#define _NRF24L01P_RF_SETUP_RF_PWR_0DBM          (0x3<<1)
+#define _NRF24L01P_RF_SETUP_RF_PWR_MINUS_6DBM    (0x2<<1)
+#define _NRF24L01P_RF_SETUP_RF_PWR_MINUS_12DBM   (0x1<<1)
+#define _NRF24L01P_RF_SETUP_RF_PWR_MINUS_18DBM   (0x0<<1)
+
+#define _NRF24L01P_RF_SETUP_RF_DR_HIGH_BIT       (1 << 3)
+#define _NRF24L01P_RF_SETUP_RF_DR_LOW_BIT        (1 << 5)
+#define _NRF24L01P_RF_SETUP_RF_DR_MASK           (_NRF24L01P_RF_SETUP_RF_DR_LOW_BIT|_NRF24L01P_RF_SETUP_RF_DR_HIGH_BIT)
+#define _NRF24L01P_RF_SETUP_RF_DR_250KBPS        (_NRF24L01P_RF_SETUP_RF_DR_LOW_BIT)
+#define _NRF24L01P_RF_SETUP_RF_DR_1MBPS          (0)
+#define _NRF24L01P_RF_SETUP_RF_DR_2MBPS          (_NRF24L01P_RF_SETUP_RF_DR_HIGH_BIT)
+
+// STATUS register:
+#define _NRF24L01P_STATUS_TX_FULL        (1<<0)
+#define _NRF24L01P_STATUS_RX_P_NO        (0x7<<1)
+#define _NRF24L01P_STATUS_MAX_RT         (1<<4)
+#define _NRF24L01P_STATUS_TX_DS          (1<<5)
+#define _NRF24L01P_STATUS_RX_DR          (1<<6)
+
+// RX_PW_P0..RX_PW_P5 registers:
+#define _NRF24L01P_RX_PW_Px_MASK         0x3F
+
+#define _NRF24L01P_TIMING_Tundef2pd_us     100000   // 100mS
+#define _NRF24L01P_TIMING_Tstby2a_us          130   // 130uS
+#define _NRF24L01P_TIMING_Thce_us              10   //  10uS
+#define _NRF24L01P_TIMING_Tpd2stby_us        4500   // 4.5mS worst case
+#define _NRF24L01P_TIMING_Tpece2csn_us          4   //   4uS
+
+/**
+ * Methods
+ */
+
+nRF24L01P::nRF24L01P(PinName mosi, 
+                     PinName miso, 
+                     PinName sck, 
+                     PinName csn,
+                     PinName ce,
+                     PinName irq) : spi_(mosi, miso, sck), nCS_(csn), ce_(ce), nIRQ_(irq) {
+
+    mode = _NRF24L01P_MODE_UNKNOWN;
+
+    disable();
+
+    nCS_ = 1;
+
+    spi_.frequency(_NRF24L01P_SPI_MAX_DATA_RATE/5);     // 2Mbit, 1/5th the maximum transfer rate for the SPI bus
+    spi_.format(8,0);                                   // 8-bit, ClockPhase = 0, ClockPolarity = 0
+
+    wait_us(_NRF24L01P_TIMING_Tundef2pd_us);    // Wait for Power-on reset
+
+    setRegister(_NRF24L01P_REG_CONFIG, 0); // Power Down
+
+    setRegister(_NRF24L01P_REG_STATUS, _NRF24L01P_STATUS_MAX_RT|_NRF24L01P_STATUS_TX_DS|_NRF24L01P_STATUS_RX_DR);   // Clear any pending interrupts
+
+    //
+    // Setup default configuration
+    //
+    disableAllRxPipes();
+    setRfFrequency();
+    setRfOutputPower();
+    setAirDataRate();
+    setCrcWidth();
+    setTxAddress();
+    setRxAddress();
+    disableAutoAcknowledge();
+    disableAutoRetransmit();
+    setTransferSize();
+
+    mode = _NRF24L01P_MODE_POWER_DOWN;
+
+}
+
+
+void nRF24L01P::powerUp(void) {
+
+    int config = getRegister(_NRF24L01P_REG_CONFIG);
+
+    config |= _NRF24L01P_CONFIG_PWR_UP;
+
+    setRegister(_NRF24L01P_REG_CONFIG, config);
+
+    // Wait until the nRF24L01+ powers up
+    wait_us( _NRF24L01P_TIMING_Tpd2stby_us );
+
+    mode = _NRF24L01P_MODE_STANDBY;
+
+}
+
+
+void nRF24L01P::powerDown(void) {
+
+    int config = getRegister(_NRF24L01P_REG_CONFIG);
+
+    config &= ~_NRF24L01P_CONFIG_PWR_UP;
+
+    setRegister(_NRF24L01P_REG_CONFIG, config);
+
+    // Wait until the nRF24L01+ powers down
+    wait_us( _NRF24L01P_TIMING_Tpd2stby_us );    // This *may* not be necessary (no timing is shown in the Datasheet), but just to be safe
+
+    mode = _NRF24L01P_MODE_POWER_DOWN;
+
+}
+
+
+void nRF24L01P::setReceiveMode(void) {
+
+    if ( _NRF24L01P_MODE_POWER_DOWN == mode ) powerUp();
+
+    int config = getRegister(_NRF24L01P_REG_CONFIG);
+
+    config |= _NRF24L01P_CONFIG_PRIM_RX;
+
+    setRegister(_NRF24L01P_REG_CONFIG, config);
+
+    mode = _NRF24L01P_MODE_RX;
+
+}
+
+
+void nRF24L01P::setTransmitMode(void) {
+
+    if ( _NRF24L01P_MODE_POWER_DOWN == mode ) powerUp();
+
+    int config = getRegister(_NRF24L01P_REG_CONFIG);
+
+    config &= ~_NRF24L01P_CONFIG_PRIM_RX;
+
+    setRegister(_NRF24L01P_REG_CONFIG, config);
+
+    mode = _NRF24L01P_MODE_TX;
+
+}
+
+
+void nRF24L01P::enable(void) {
+
+    ce_ = 1;
+    wait_us( _NRF24L01P_TIMING_Tpece2csn_us );
+
+}
+
+
+void nRF24L01P::disable(void) {
+
+    ce_ = 0;
+
+}
+
+void nRF24L01P::setRfFrequency(int frequency) {
+
+    if ( ( frequency < NRF24L01P_MIN_RF_FREQUENCY ) || ( frequency > NRF24L01P_MAX_RF_FREQUENCY ) ) {
+
+        error( "nRF24L01P: Invalid RF Frequency setting %d\r\n", frequency );
+        return;
+
+    }
+
+    int channel = ( frequency - NRF24L01P_MIN_RF_FREQUENCY ) & 0x7F;
+
+    setRegister(_NRF24L01P_REG_RF_CH, channel);
+
+}
+
+
+int nRF24L01P::getRfFrequency(void) {
+
+    int channel = getRegister(_NRF24L01P_REG_RF_CH) & 0x7F;
+
+    return ( channel + NRF24L01P_MIN_RF_FREQUENCY );
+
+}
+
+
+void nRF24L01P::setRfOutputPower(int power) {
+
+    int rfSetup = getRegister(_NRF24L01P_REG_RF_SETUP) & ~_NRF24L01P_RF_SETUP_RF_PWR_MASK;
+
+    switch ( power ) {
+
+        case NRF24L01P_TX_PWR_ZERO_DB:
+            rfSetup |= _NRF24L01P_RF_SETUP_RF_PWR_0DBM;
+            break;
+
+        case NRF24L01P_TX_PWR_MINUS_6_DB:
+            rfSetup |= _NRF24L01P_RF_SETUP_RF_PWR_MINUS_6DBM;
+            break;
+
+        case NRF24L01P_TX_PWR_MINUS_12_DB:
+            rfSetup |= _NRF24L01P_RF_SETUP_RF_PWR_MINUS_12DBM;
+            break;
+
+        case NRF24L01P_TX_PWR_MINUS_18_DB:
+            rfSetup |= _NRF24L01P_RF_SETUP_RF_PWR_MINUS_18DBM;
+            break;
+
+        default:
+            error( "nRF24L01P: Invalid RF Output Power setting %d\r\n", power );
+            return;
+
+    }
+
+    setRegister(_NRF24L01P_REG_RF_SETUP, rfSetup);
+
+}
+
+
+int nRF24L01P::getRfOutputPower(void) {
+
+    int rfPwr = getRegister(_NRF24L01P_REG_RF_SETUP) & _NRF24L01P_RF_SETUP_RF_PWR_MASK;
+
+    switch ( rfPwr ) {
+
+        case _NRF24L01P_RF_SETUP_RF_PWR_0DBM:
+            return NRF24L01P_TX_PWR_ZERO_DB;
+
+        case _NRF24L01P_RF_SETUP_RF_PWR_MINUS_6DBM:
+            return NRF24L01P_TX_PWR_MINUS_6_DB;
+
+        case _NRF24L01P_RF_SETUP_RF_PWR_MINUS_12DBM:
+            return NRF24L01P_TX_PWR_MINUS_12_DB;
+
+        case _NRF24L01P_RF_SETUP_RF_PWR_MINUS_18DBM:
+            return NRF24L01P_TX_PWR_MINUS_18_DB;
+
+        default:
+            error( "nRF24L01P: Unknown RF Output Power value %d\r\n", rfPwr );
+            return 0;
+
+    }
+}
+
+
+void nRF24L01P::setAirDataRate(int rate) {
+
+    int rfSetup = getRegister(_NRF24L01P_REG_RF_SETUP) & ~_NRF24L01P_RF_SETUP_RF_DR_MASK;
+
+    switch ( rate ) {
+
+        case NRF24L01P_DATARATE_250_KBPS:
+            rfSetup |= _NRF24L01P_RF_SETUP_RF_DR_250KBPS;
+            break;
+
+        case NRF24L01P_DATARATE_1_MBPS:
+            rfSetup |= _NRF24L01P_RF_SETUP_RF_DR_1MBPS;
+            break;
+
+        case NRF24L01P_DATARATE_2_MBPS:
+            rfSetup |= _NRF24L01P_RF_SETUP_RF_DR_2MBPS;
+            break;
+
+        default:
+            error( "nRF24L01P: Invalid Air Data Rate setting %d\r\n", rate );
+            return;
+
+    }
+
+    setRegister(_NRF24L01P_REG_RF_SETUP, rfSetup);
+
+}
+
+
+int nRF24L01P::getAirDataRate(void) {
+
+    int rfDataRate = getRegister(_NRF24L01P_REG_RF_SETUP) & _NRF24L01P_RF_SETUP_RF_DR_MASK;
+
+    switch ( rfDataRate ) {
+
+        case _NRF24L01P_RF_SETUP_RF_DR_250KBPS:
+            return NRF24L01P_DATARATE_250_KBPS;
+
+        case _NRF24L01P_RF_SETUP_RF_DR_1MBPS:
+            return NRF24L01P_DATARATE_1_MBPS;
+
+        case _NRF24L01P_RF_SETUP_RF_DR_2MBPS:
+            return NRF24L01P_DATARATE_2_MBPS;
+
+        default:
+            error( "nRF24L01P: Unknown Air Data Rate value %d\r\n", rfDataRate );
+            return 0;
+
+    }
+}
+
+
+void nRF24L01P::setCrcWidth(int width) {
+
+    int config = getRegister(_NRF24L01P_REG_CONFIG) & ~_NRF24L01P_CONFIG_CRC_MASK;
+
+    switch ( width ) {
+
+        case NRF24L01P_CRC_NONE:
+            config |= _NRF24L01P_CONFIG_CRC_NONE;
+            break;
+
+        case NRF24L01P_CRC_8_BIT:
+            config |= _NRF24L01P_CONFIG_CRC_8BIT;
+            break;
+
+        case NRF24L01P_CRC_16_BIT:
+            config |= _NRF24L01P_CONFIG_CRC_16BIT;
+            break;
+
+        default:
+            error( "nRF24L01P: Invalid CRC Width setting %d\r\n", width );
+            return;
+
+    }
+
+    setRegister(_NRF24L01P_REG_CONFIG, config);
+
+}
+
+
+int nRF24L01P::getCrcWidth(void) {
+
+    int crcWidth = getRegister(_NRF24L01P_REG_CONFIG) & _NRF24L01P_CONFIG_CRC_MASK;
+
+    switch ( crcWidth ) {
+
+        case _NRF24L01P_CONFIG_CRC_NONE:
+            return NRF24L01P_CRC_NONE;
+
+        case _NRF24L01P_CONFIG_CRC_8BIT:
+            return NRF24L01P_CRC_8_BIT;
+
+        case _NRF24L01P_CONFIG_CRC_16BIT:
+            return NRF24L01P_CRC_16_BIT;
+
+        default:
+            error( "nRF24L01P: Unknown CRC Width value %d\r\n", crcWidth );
+            return 0;
+
+    }
+}
+
+
+void nRF24L01P::setTransferSize(int size, int pipe) {
+
+    if ( ( pipe < NRF24L01P_PIPE_P0 ) || ( pipe > NRF24L01P_PIPE_P5 ) ) {
+
+        error( "nRF24L01P: Invalid Transfer Size pipe number %d\r\n", pipe );
+        return;
+
+    }
+
+    if ( ( size < 0 ) || ( size > _NRF24L01P_RX_FIFO_SIZE ) ) {
+
+        error( "nRF24L01P: Invalid Transfer Size setting %d\r\n", size );
+        return;
+
+    }
+
+    int rxPwPxRegister = _NRF24L01P_REG_RX_PW_P0 + ( pipe - NRF24L01P_PIPE_P0 );
+
+    setRegister(rxPwPxRegister, ( size & _NRF24L01P_RX_PW_Px_MASK ) );
+
+}
+
+
+int nRF24L01P::getTransferSize(int pipe) {
+
+    if ( ( pipe < NRF24L01P_PIPE_P0 ) || ( pipe > NRF24L01P_PIPE_P5 ) ) {
+
+        error( "nRF24L01P: Invalid Transfer Size pipe number %d\r\n", pipe );
+        return 0;
+
+    }
+
+    int rxPwPxRegister = _NRF24L01P_REG_RX_PW_P0 + ( pipe - NRF24L01P_PIPE_P0 );
+
+    int size = getRegister(rxPwPxRegister);
+    
+    return ( size & _NRF24L01P_RX_PW_Px_MASK );
+
+}
+
+
+void nRF24L01P::disableAllRxPipes(void) {
+
+    setRegister(_NRF24L01P_REG_EN_RXADDR, _NRF24L01P_EN_RXADDR_NONE);
+
+}
+
+
+void nRF24L01P::disableAutoAcknowledge(void) {
+
+    setRegister(_NRF24L01P_REG_EN_AA, _NRF24L01P_EN_AA_NONE);
+
+}
+
+
+void nRF24L01P::enableAutoAcknowledge(int pipe) {
+
+    if ( ( pipe < NRF24L01P_PIPE_P0 ) || ( pipe > NRF24L01P_PIPE_P5 ) ) {
+
+        error( "nRF24L01P: Invalid Enable AutoAcknowledge pipe number %d\r\n", pipe );
+        return;
+
+    }
+
+    int enAA = getRegister(_NRF24L01P_REG_EN_AA);
+
+    enAA |= ( 1 << (pipe - NRF24L01P_PIPE_P0) );
+
+    setRegister(_NRF24L01P_REG_EN_AA, enAA);
+
+}
+
+
+void nRF24L01P::disableAutoRetransmit(void) {
+
+    setRegister(_NRF24L01P_REG_SETUP_RETR, _NRF24L01P_SETUP_RETR_NONE);
+
+}
+
+void nRF24L01P::setRxAddress(unsigned long long address, int width, int pipe) {
+
+    if ( ( pipe < NRF24L01P_PIPE_P0 ) || ( pipe > NRF24L01P_PIPE_P5 ) ) {
+
+        error( "nRF24L01P: Invalid setRxAddress pipe number %d\r\n", pipe );
+        return;
+
+    }
+
+    if ( ( pipe == NRF24L01P_PIPE_P0 ) || ( pipe == NRF24L01P_PIPE_P1 ) ) {
+
+        int setupAw = getRegister(_NRF24L01P_REG_SETUP_AW) & ~_NRF24L01P_SETUP_AW_AW_MASK;
+    
+        switch ( width ) {
+    
+            case 3:
+                setupAw |= _NRF24L01P_SETUP_AW_AW_3BYTE;
+                break;
+    
+            case 4:
+                setupAw |= _NRF24L01P_SETUP_AW_AW_4BYTE;
+                break;
+    
+            case 5:
+                setupAw |= _NRF24L01P_SETUP_AW_AW_5BYTE;
+                break;
+    
+            default:
+                error( "nRF24L01P: Invalid setRxAddress width setting %d\r\n", width );
+                return;
+    
+        }
+    
+        setRegister(_NRF24L01P_REG_SETUP_AW, setupAw);
+
+    } else {
+    
+        width = 1;
+    
+    }
+
+    int rxAddrPxRegister = _NRF24L01P_REG_RX_ADDR_P0 + ( pipe - NRF24L01P_PIPE_P0 );
+
+    int cn = (_NRF24L01P_SPI_CMD_WR_REG | (rxAddrPxRegister & _NRF24L01P_REG_ADDRESS_MASK));
+
+    nCS_ = 0;
+
+    int status = spi_.write(cn);
+
+    while ( width-- > 0 ) {
+
+        //
+        // LSByte first
+        //
+        spi_.write((int) (address & 0xFF));
+        address >>= 8;
+
+    }
+
+    nCS_ = 1;
+
+    int enRxAddr = getRegister(_NRF24L01P_REG_EN_RXADDR);
+
+    enRxAddr |= (1 << ( pipe - NRF24L01P_PIPE_P0 ) );
+
+    setRegister(_NRF24L01P_REG_EN_RXADDR, enRxAddr);
+}
+
+/*
+ * This version of setRxAddress is just a wrapper for the version that takes 'long long's,
+ *  in case the main code doesn't want to deal with long long's.
+ */
+void nRF24L01P::setRxAddress(unsigned long msb_address, unsigned long lsb_address, int width, int pipe) {
+
+    unsigned long long address = ( ( (unsigned long long) msb_address ) << 32 ) | ( ( (unsigned long long) lsb_address ) << 0 );
+
+    setRxAddress(address, width, pipe);
+
+}
+
+
+/*
+ * This version of setTxAddress is just a wrapper for the version that takes 'long long's,
+ *  in case the main code doesn't want to deal with long long's.
+ */
+void nRF24L01P::setTxAddress(unsigned long msb_address, unsigned long lsb_address, int width) {
+
+    unsigned long long address = ( ( (unsigned long long) msb_address ) << 32 ) | ( ( (unsigned long long) lsb_address ) << 0 );
+
+    setTxAddress(address, width);
+
+}
+
+
+void nRF24L01P::setTxAddress(unsigned long long address, int width) {
+
+    int setupAw = getRegister(_NRF24L01P_REG_SETUP_AW) & ~_NRF24L01P_SETUP_AW_AW_MASK;
+
+    switch ( width ) {
+
+        case 3:
+            setupAw |= _NRF24L01P_SETUP_AW_AW_3BYTE;
+            break;
+
+        case 4:
+            setupAw |= _NRF24L01P_SETUP_AW_AW_4BYTE;
+            break;
+
+        case 5:
+            setupAw |= _NRF24L01P_SETUP_AW_AW_5BYTE;
+            break;
+
+        default:
+            error( "nRF24L01P: Invalid setTxAddress width setting %d\r\n", width );
+            return;
+
+    }
+
+    setRegister(_NRF24L01P_REG_SETUP_AW, setupAw);
+
+    int cn = (_NRF24L01P_SPI_CMD_WR_REG | (_NRF24L01P_REG_TX_ADDR & _NRF24L01P_REG_ADDRESS_MASK));
+
+    nCS_ = 0;
+
+    int status = spi_.write(cn);
+
+    while ( width-- > 0 ) {
+
+        //
+        // LSByte first
+        //
+        spi_.write((int) (address & 0xFF));
+        address >>= 8;
+
+    }
+
+    nCS_ = 1;
+
+}
+
+
+unsigned long long nRF24L01P::getRxAddress(int pipe) {
+
+    if ( ( pipe < NRF24L01P_PIPE_P0 ) || ( pipe > NRF24L01P_PIPE_P5 ) ) {
+
+        error( "nRF24L01P: Invalid setRxAddress pipe number %d\r\n", pipe );
+        return 0;
+
+    }
+
+    int width;
+
+    if ( ( pipe == NRF24L01P_PIPE_P0 ) || ( pipe == NRF24L01P_PIPE_P1 ) ) {
+
+        int setupAw = getRegister(_NRF24L01P_REG_SETUP_AW) & _NRF24L01P_SETUP_AW_AW_MASK;
+
+        switch ( setupAw ) {
+
+            case _NRF24L01P_SETUP_AW_AW_3BYTE:
+                width = 3;
+                break;
+
+            case _NRF24L01P_SETUP_AW_AW_4BYTE:
+                width = 4;
+                break;
+
+            case _NRF24L01P_SETUP_AW_AW_5BYTE:
+                width = 5;
+                break;
+
+            default:
+                error( "nRF24L01P: Unknown getRxAddress width value %d\r\n", setupAw );
+                return 0;
+
+        }
+
+    } else {
+
+        width = 1;
+
+    }
+
+    int rxAddrPxRegister = _NRF24L01P_REG_RX_ADDR_P0 + ( pipe - NRF24L01P_PIPE_P0 );
+
+    int cn = (_NRF24L01P_SPI_CMD_RD_REG | (rxAddrPxRegister & _NRF24L01P_REG_ADDRESS_MASK));
+
+    unsigned long long address = 0;
+
+    nCS_ = 0;
+
+    int status = spi_.write(cn);
+
+    for ( int i=0; i<width; i++ ) {
+
+        //
+        // LSByte first
+        //
+        address |= ( ( (unsigned long long)( spi_.write(_NRF24L01P_SPI_CMD_NOP) & 0xFF ) ) << (i*8) );
+
+    }
+
+    nCS_ = 1;
+
+    if ( !( ( pipe == NRF24L01P_PIPE_P0 ) || ( pipe == NRF24L01P_PIPE_P1 ) ) ) {
+
+        address |= ( getRxAddress(NRF24L01P_PIPE_P1) & ~((unsigned long long) 0xFF) );
+
+    }
+
+    return address;
+
+}
+
+    
+unsigned long long nRF24L01P::getTxAddress(void) {
+
+    int setupAw = getRegister(_NRF24L01P_REG_SETUP_AW) & _NRF24L01P_SETUP_AW_AW_MASK;
+
+    int width;
+
+    switch ( setupAw ) {
+
+        case _NRF24L01P_SETUP_AW_AW_3BYTE:
+            width = 3;
+            break;
+
+        case _NRF24L01P_SETUP_AW_AW_4BYTE:
+            width = 4;
+            break;
+
+        case _NRF24L01P_SETUP_AW_AW_5BYTE:
+            width = 5;
+            break;
+
+        default:
+            error( "nRF24L01P: Unknown getTxAddress width value %d\r\n", setupAw );
+            return 0;
+
+    }
+
+    int cn = (_NRF24L01P_SPI_CMD_RD_REG | (_NRF24L01P_REG_TX_ADDR & _NRF24L01P_REG_ADDRESS_MASK));
+
+    unsigned long long address = 0;
+
+    nCS_ = 0;
+
+    int status = spi_.write(cn);
+
+    for ( int i=0; i<width; i++ ) {
+
+        //
+        // LSByte first
+        //
+        address |= ( ( (unsigned long long)( spi_.write(_NRF24L01P_SPI_CMD_NOP) & 0xFF ) ) << (i*8) );
+
+    }
+
+    nCS_ = 1;
+
+    return address;
+}
+
+
+bool nRF24L01P::readable(int pipe) {
+
+    if ( ( pipe < NRF24L01P_PIPE_P0 ) || ( pipe > NRF24L01P_PIPE_P5 ) ) {
+
+        error( "nRF24L01P: Invalid readable pipe number %d\r\n", pipe );
+        return false;
+
+    }
+
+    int status = getStatusRegister();
+
+    return ( ( status & _NRF24L01P_STATUS_RX_DR ) && ( ( ( status & _NRF24L01P_STATUS_RX_P_NO ) >> 1 ) == ( pipe & 0x7 ) ) );
+
+}
+
+
+int nRF24L01P::write(int pipe, char *data, int count) {
+
+    // Note: the pipe number is ignored in a Transmit / write
+
+    //
+    // Save the CE state
+    //
+    int originalCe = ce_;
+    disable();
+
+    if ( count <= 0 ) return 0;
+
+    if ( count > _NRF24L01P_TX_FIFO_SIZE ) count = _NRF24L01P_TX_FIFO_SIZE;
+
+    // Clear the Status bit
+    setRegister(_NRF24L01P_REG_STATUS, _NRF24L01P_STATUS_TX_DS);
+    
+    nCS_ = 0;
+
+    int status = spi_.write(_NRF24L01P_SPI_CMD_WR_TX_PAYLOAD);
+
+    for ( int i = 0; i < count; i++ ) {
+
+        spi_.write(*data++);
+
+    }
+
+    nCS_ = 1;
+
+    int originalMode = mode;
+    setTransmitMode();
+
+    enable();
+    wait_us(_NRF24L01P_TIMING_Thce_us);
+    disable();
+
+ //   while ( !( getStatusRegister() & _NRF24L01P_STATUS_TX_DS ) ) {
+
+        // Wait for the transfer to complete
+
+   // }
+
+    // Clear the Status bit
+    setRegister(_NRF24L01P_REG_STATUS, _NRF24L01P_STATUS_TX_DS);
+
+    if ( originalMode == _NRF24L01P_MODE_RX ) {
+
+        setReceiveMode();
+
+    }
+
+    ce_ = originalCe;
+    wait_us( _NRF24L01P_TIMING_Tpece2csn_us );
+
+    return count;
+
+}
+
+
+int nRF24L01P::read(int pipe, char *data, int count) {
+
+    if ( ( pipe < NRF24L01P_PIPE_P0 ) || ( pipe > NRF24L01P_PIPE_P5 ) ) {
+
+        error( "nRF24L01P: Invalid read pipe number %d\r\n", pipe );
+        return -1;
+
+    }
+
+    if ( count <= 0 ) return 0;
+
+    if ( count > _NRF24L01P_RX_FIFO_SIZE ) count = _NRF24L01P_RX_FIFO_SIZE;
+
+    if ( readable(pipe) ) {
+
+        nCS_ = 0;
+
+        int status = spi_.write(_NRF24L01P_SPI_CMD_R_RX_PL_WID);
+
+        int rxPayloadWidth = spi_.write(_NRF24L01P_SPI_CMD_NOP);
+        
+        nCS_ = 1;
+
+        if ( ( rxPayloadWidth < 0 ) || ( rxPayloadWidth > _NRF24L01P_RX_FIFO_SIZE ) ) {
+    
+            // Received payload error: need to flush the FIFO
+
+            nCS_ = 0;
+    
+            int status = spi_.write(_NRF24L01P_SPI_CMD_FLUSH_RX);
+    
+            int rxPayloadWidth = spi_.write(_NRF24L01P_SPI_CMD_NOP);
+            
+            nCS_ = 1;
+            
+            //
+            // At this point, we should retry the reception,
+            //  but for now we'll just fall through...
+            //
+
+        } else {
+
+            if ( rxPayloadWidth < count ) count = rxPayloadWidth;
+
+            nCS_ = 0;
+        
+            int status = spi_.write(_NRF24L01P_SPI_CMD_RD_RX_PAYLOAD);
+        
+            for ( int i = 0; i < count; i++ ) {
+        
+                *data++ = spi_.write(_NRF24L01P_SPI_CMD_NOP);
+        
+            }
+
+            nCS_ = 1;
+
+            // Clear the Status bit
+            setRegister(_NRF24L01P_REG_STATUS, _NRF24L01P_STATUS_RX_DR);
+
+            return count;
+
+        }
+
+    } else {
+
+        //
+        // What should we do if there is no 'readable' data?
+        //  We could wait for data to arrive, but for now, we'll
+        //  just return with no data.
+        //
+        return 0;
+
+    }
+
+    //
+    // We get here because an error condition occured;
+    //  We could wait for data to arrive, but for now, we'll
+    //  just return with no data.
+    //
+    return -1;
+
+}
+
+void nRF24L01P::setRegister(int regAddress, int regData) {
+
+    //
+    // Save the CE state
+    //
+    int originalCe = ce_;
+    disable();
+
+    int cn = (_NRF24L01P_SPI_CMD_WR_REG | (regAddress & _NRF24L01P_REG_ADDRESS_MASK));
+
+    nCS_ = 0;
+
+    int status = spi_.write(cn);
+
+    spi_.write(regData & 0xFF);
+
+    nCS_ = 1;
+
+    ce_ = originalCe;
+    wait_us( _NRF24L01P_TIMING_Tpece2csn_us );
+
+}
+
+
+int nRF24L01P::getRegister(int regAddress) {
+
+    int cn = (_NRF24L01P_SPI_CMD_RD_REG | (regAddress & _NRF24L01P_REG_ADDRESS_MASK));
+
+    nCS_ = 0;
+
+    int status = spi_.write(cn);
+
+    int dn = spi_.write(_NRF24L01P_SPI_CMD_NOP);
+
+    nCS_ = 1;
+
+    return dn;
+
+}
+
+int nRF24L01P::getStatusRegister(void) {
+
+    nCS_ = 0;
+
+    int status = spi_.write(_NRF24L01P_SPI_CMD_NOP);
+
+    nCS_ = 1;
+
+    return status;
+
+}
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/nRF24L01P.h	Thu Feb 02 12:21:11 2017 +0000
@@ -0,0 +1,348 @@
+/**
+ * @file nRF24L01P.h
+ *
+ * @author Owen Edwards
+ * 
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 Owen Edwards
+ *
+ *    This program is free software: you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation, either version 3 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License
+ *    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * @section DESCRIPTION
+ *
+ * nRF24L01+ Single Chip 2.4GHz Transceiver from Nordic Semiconductor.
+ *
+ * Datasheet:
+ *
+ * http://www.nordicsemi.no/files/Product/data_sheet/nRF24L01P_Product_Specification_1_0.pdf
+ */
+
+#ifndef __NRF24L01P_H__
+#define __NRF24L01P_H__
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+
+/**
+ * Defines
+ */
+#define NRF24L01P_TX_PWR_ZERO_DB         0
+#define NRF24L01P_TX_PWR_MINUS_6_DB     -6
+#define NRF24L01P_TX_PWR_MINUS_12_DB   -12
+#define NRF24L01P_TX_PWR_MINUS_18_DB   -18
+
+#define NRF24L01P_DATARATE_250_KBPS    250
+#define NRF24L01P_DATARATE_1_MBPS     1000
+#define NRF24L01P_DATARATE_2_MBPS     2000
+
+#define NRF24L01P_CRC_NONE               0
+#define NRF24L01P_CRC_8_BIT              8
+#define NRF24L01P_CRC_16_BIT            16
+
+#define NRF24L01P_MIN_RF_FREQUENCY    2400
+#define NRF24L01P_MAX_RF_FREQUENCY    2525
+
+#define NRF24L01P_PIPE_P0                0
+#define NRF24L01P_PIPE_P1                1
+#define NRF24L01P_PIPE_P2                2
+#define NRF24L01P_PIPE_P3                3
+#define NRF24L01P_PIPE_P4                4
+#define NRF24L01P_PIPE_P5                5
+
+/**
+* Default setup for the nRF24L01+, based on the Sparkfun "Nordic Serial Interface Board"
+*  for evaluation (http://www.sparkfun.com/products/9019)
+*/
+#define DEFAULT_NRF24L01P_ADDRESS       ((unsigned long long) 0xE7E7E7E7E7 )
+#define DEFAULT_NRF24L01P_ADDRESS_WIDTH  5
+#define DEFAULT_NRF24L01P_CRC            NRF24L01P_CRC_8_BIT
+#define DEFAULT_NRF24L01P_RF_FREQUENCY  (NRF24L01P_MIN_RF_FREQUENCY + 2)
+#define DEFAULT_NRF24L01P_DATARATE       NRF24L01P_DATARATE_1_MBPS
+#define DEFAULT_NRF24L01P_TX_PWR         NRF24L01P_TX_PWR_ZERO_DB
+#define DEFAULT_NRF24L01P_TRANSFER_SIZE  4
+
+/**
+ * nRF24L01+ Single Chip 2.4GHz Transceiver from Nordic Semiconductor.
+ */
+class nRF24L01P {
+
+public:
+
+    /**
+     * Constructor.
+     *
+     * @param mosi mbed pin to use for MOSI line of SPI interface.
+     * @param miso mbed pin to use for MISO line of SPI interface.
+     * @param sck mbed pin to use for SCK line of SPI interface.
+     * @param csn mbed pin to use for not chip select line of SPI interface.
+     * @param ce mbed pin to use for the chip enable line.
+     * @param irq mbed pin to use for the interrupt request line.
+     */
+    nRF24L01P(PinName mosi, PinName miso, PinName sck, PinName csn, PinName ce, PinName irq = NC);
+
+    /**
+     * Set the RF frequency.
+     *
+     * @param frequency the frequency of RF transmission in MHz (2400..2525).
+     */
+    void setRfFrequency(int frequency = DEFAULT_NRF24L01P_RF_FREQUENCY);
+
+    /**
+     * Get the RF frequency.
+     *
+     * @return the frequency of RF transmission in MHz (2400..2525).
+     */
+    int getRfFrequency(void);
+
+    /**
+     * Set the RF output power.
+     *
+     * @param power the RF output power in dBm (0, -6, -12 or -18).
+     */
+    void setRfOutputPower(int power = DEFAULT_NRF24L01P_TX_PWR);
+
+    /**
+     * Get the RF output power.
+     *
+     * @return the RF output power in dBm (0, -6, -12 or -18).
+     */
+    int getRfOutputPower(void);
+
+    /**
+     * Set the Air data rate.
+     *
+     * @param rate the air data rate in kbps (250, 1M or 2M).
+     */
+    void setAirDataRate(int rate = DEFAULT_NRF24L01P_DATARATE);
+
+    /**
+     * Get the Air data rate.
+     *
+     * @return the air data rate in kbps (250, 1M or 2M).
+     */
+    int getAirDataRate(void);
+
+    /**
+     * Set the CRC width.
+     *
+     * @param width the number of bits for the CRC (0, 8 or 16).
+     */
+    void setCrcWidth(int width = DEFAULT_NRF24L01P_CRC);
+
+    /**
+     * Get the CRC width.
+     *
+     * @return the number of bits for the CRC (0, 8 or 16).
+     */
+    int getCrcWidth(void);
+
+    /**
+     * Set the Receive address.
+     *
+     * @param address address associated with the particular pipe
+     * @param width width of the address in bytes (3..5)
+     * @param pipe pipe to associate the address with (0..5, default 0)
+     *
+     * Note that Pipes 0 & 1 have 3, 4 or 5 byte addresses,
+     *  while Pipes 2..5 only use the lowest byte (bits 7..0) of the
+     *  address provided here, and use 2, 3 or 4 bytes from Pipe 1's address.
+     *  The width parameter is ignored for Pipes 2..5.
+     */
+    void setRxAddress(unsigned long long address = DEFAULT_NRF24L01P_ADDRESS, int width = DEFAULT_NRF24L01P_ADDRESS_WIDTH, int pipe = NRF24L01P_PIPE_P0);
+
+    void setRxAddress(unsigned long msb_address, unsigned long lsb_address, int width, int pipe = NRF24L01P_PIPE_P0);
+
+    /**
+     * Set the Transmit address.
+     *
+     * @param address address for transmission
+     * @param width width of the address in bytes (3..5)
+     *
+     * Note that the address width is shared with the Receive pipes,
+     *  so a change to that address width affect transmissions.
+     */
+    void setTxAddress(unsigned long long address = DEFAULT_NRF24L01P_ADDRESS, int width = DEFAULT_NRF24L01P_ADDRESS_WIDTH);
+
+    void setTxAddress(unsigned long msb_address, unsigned long lsb_address, int width);
+
+    /**
+     * Get the Receive address.
+     *
+     * @param pipe pipe to get the address from (0..5, default 0)
+     * @return the address associated with the particular pipe
+     */
+    unsigned long long getRxAddress(int pipe = NRF24L01P_PIPE_P0);
+
+    /**
+     * Get the Transmit address.
+     *
+     * @return address address for transmission
+     */
+    unsigned long long getTxAddress(void);
+
+    /**
+     * Set the transfer size.
+     *
+     * @param size the size of the transfer, in bytes (1..32)
+     * @param pipe pipe for the transfer (0..5, default 0)
+     */
+    void setTransferSize(int size = DEFAULT_NRF24L01P_TRANSFER_SIZE, int pipe = NRF24L01P_PIPE_P0);
+
+    /**
+     * Get the transfer size.
+     *
+     * @return the size of the transfer, in bytes (1..32).
+     */
+    int getTransferSize(int pipe = NRF24L01P_PIPE_P0);
+
+
+    /**
+     * Get the RPD (Received Power Detector) state.
+     *
+     * @return true if the received power exceeded -64dBm
+     */
+    bool getRPD(void);
+
+    /**
+     * Put the nRF24L01+ into Receive mode
+     */
+    void setReceiveMode(void);
+
+    /**
+     * Put the nRF24L01+ into Transmit mode
+     */
+    void setTransmitMode(void);
+
+    /**
+     * Power up the nRF24L01+ into Standby mode
+     */
+    void powerUp(void);
+
+    /**
+     * Power down the nRF24L01+ into Power Down mode
+     */
+    void powerDown(void);
+
+    /**
+     * Enable the nRF24L01+ to Receive or Transmit (using the CE pin)
+     */
+    void enable(void);
+
+    /**
+     * Disable the nRF24L01+ to Receive or Transmit (using the CE pin)
+     */
+    void disable(void);
+
+    /**
+     * Transmit data
+     *
+     * @param pipe is ignored (included for consistency with file write routine)
+     * @param data pointer to an array of bytes to write
+     * @param count the number of bytes to send (1..32)
+     * @return the number of bytes actually written, or -1 for an error
+     */
+    int write(int pipe, char *data, int count);
+    
+    /**
+     * Receive data
+     *
+     * @param pipe the receive pipe to get data from
+     * @param data pointer to an array of bytes to store the received data
+     * @param count the number of bytes to receive (1..32)
+     * @return the number of bytes actually received, 0 if none are received, or -1 for an error
+     */
+    int read(int pipe, char *data, int count);
+
+    /**
+     * Determine if there is data available to read
+     *
+     * @param pipe the receive pipe to check for data
+     * @return true if the is data waiting in the given pipe
+     */
+    bool readable(int pipe = NRF24L01P_PIPE_P0);
+
+    /**
+     * Disable all receive pipes
+     *
+     * Note: receive pipes are enabled when their address is set.
+     */
+    void disableAllRxPipes(void);
+    
+    /**
+     * Disable AutoAcknowledge function
+     */
+    void disableAutoAcknowledge(void);
+    
+    /**
+     * Enable AutoAcknowledge function
+     *
+     * @param pipe the receive pipe
+     */
+    void enableAutoAcknowledge(int pipe = NRF24L01P_PIPE_P0);
+    
+    /**
+     * Disable AutoRetransmit function
+     */
+    void disableAutoRetransmit(void);
+    
+    /**
+     * Enable AutoRetransmit function
+     *
+     * @param delay the delay between restransmits, in uS (250uS..4000uS)
+     * @param count number of retransmits before generating an error (1..15)
+     */
+    void enableAutoRetransmit(int delay, int count);
+
+private:
+
+    /**
+     * Get the contents of an addressable register.
+     *
+     * @param regAddress address of the register
+     * @return the contents of the register
+     */
+    int getRegister(int regAddress);
+
+    /**
+     * Set the contents of an addressable register.
+     *
+     * @param regAddress address of the register
+     * @param regData data to write to the register
+     */
+    void setRegister(int regAddress, int regData);
+
+    /**
+     * Get the contents of the status register.
+     *
+     * @return the contents of the status register
+     */
+    int getStatusRegister(void);
+
+    SPI         spi_;
+    DigitalOut  nCS_;
+    DigitalOut  ce_;
+    InterruptIn nIRQ_;
+
+    int mode;
+
+};
+
+#endif /* __NRF24L01P_H__ */
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/robot.h	Thu Feb 02 12:21:11 2017 +0000
@@ -0,0 +1,1027 @@
+/** @file */
+//AUTOR: Fernando Pais
+//MAIL:  ferpais2508@gmail.com
+//DATA: 6/6/2016
+// VERSÃO 6.4.0
+//
+//Alterações: Problema de compatibilidade entre encoder e infravermelho resolvido
+//            Odometria actualizada automaticamente
+//            Valor da bateria verificado na inicialização
+//            Motores movem-se de 0 a 1000 para melhor difrenciação
+//
+
+//#include "mbed.h"
+//#include "init.h"
+//#define _USE_MATH_DEFINES
+# define M_PI           3.14159265358979323846  /* pi */
+#include <math.h>
+#include <string.h>
+#include "VCNL40x0.h"
+#include "nRF24L01P.h"
+
+void Odometria();
+
+// classes adicionais
+nRF24L01P my_nrf24l01p(PTD2, PTD3, PTD1, PTC13, PTC12, PTA13);
+VCNL40x0 VCNL40x0_Device (PTC9, PTC8, VCNL40x0_ADDRESS);
+Timeout timeout;
+
+Serial pc(PTE0,PTE1);
+I2C i2c(PTC9,PTC8);
+I2C i2c1(PTC11,PTC10);
+
+// Variables needed by the lib
+unsigned int  ProxiValue=0;
+short int prev_R=0;
+short int prev_L=0;
+long int total_R=0;
+long int total_L=0;
+long int ticks2d=0;
+long int ticks2e=0;
+float X=20;
+float Y=20;
+float AtractX = 0;
+float AtractY = 0;
+float theta=0;
+int sensor_left=0;
+int sensor_front=0;
+int sensor_right=0;
+short int flag=0;
+int IRobot=0;
+int JRobot=0;
+
+//SAIDAS DIGITAIS (normal)
+DigitalOut  q_pha_mot_rig       (PTE4,0);     //Phase Motor Right
+DigitalOut  q_sleep_mot_rig     (PTE3,0);     //Nano Sleep Motor Right
+DigitalOut  q_pha_mot_lef       (PTA17,0);    //Phase Motor Left
+DigitalOut  q_sleep_mot_lef     (PTB11,0);    //Nano Sleep Motor Left
+DigitalOut  q_pow_ena_i2c_p     (PTE2,0);     //Power Enable i2c FET P (0- enable 1-disable)
+DigitalOut  q_pow_ena_mic_p     (PTA14,0);    //Power enable Micro FET P (0- enable 1-disable)
+DigitalOut  q_pow_as5600_n      (PTC6,1);     //AS5600 Power MOSFET N (1- enable 0-disable)
+DigitalOut  q_pow_as5600_p      (PTC5,0);     //AS5600 Power MOSFET P (0- enable 1-disable)
+DigitalOut  q_pow_spi           (PTC4,0);     //SPI Power MOSFET P (0- enable 1-disable)
+DigitalOut  q_ena_mppt          (PTC0,0);     //Enable MPPT Control (0- enable 1-disable)
+DigitalOut  q_boost_ps          (PTC7,1);     //Boost Power Save (1- enable 0-disable)
+DigitalOut  q_tca9548_reset     (PTC3,1);     //Reset TCA9548 (1- enable 0-disable)
+DigitalOut  power_36khz         (PTD0,0);     //Power enable pic12f - 36khz (0- enable 1-disable)
+
+
+// ********************************************************************
+// ********************************************************************
+//DEFINIÇÃO DE ENTRADAS E SAIDAS DO ROBOT
+//ENTRADAS DIGITAIS (normal input)
+DigitalIn   i_enc_dir_rig       (PTB8);     //Encoder Right Direction
+DigitalIn   i_enc_dir_lef       (PTB9);     //Encoder Left Direction
+DigitalIn   i_micro_sd_det      (PTC16);    //MICRO SD Card Detect
+DigitalIn   i_mppt_fail         (PTE5);     //Fail MPPT Signal
+DigitalIn   i_usb_volt          (PTB10);    //USB Voltage detect
+DigitalIn   i_sup_cap_est       (PTB19);    //Supercap State Charger
+DigitalIn   i_li_ion_est        (PTB18);    //Li-ion State Charger
+
+
+// ********************************************************************
+//ENTRADAS DIGITAIS (external interrupt)
+InterruptIn i_int_mpu9250       (PTA15);    //Interrupt MPU9250
+InterruptIn i_int_isl29125      (PTA16);    //Interrupt ISL29125 Color S.
+InterruptIn i_mic_f_l           (PTD7);     //Interrupt Comp Micro F L
+InterruptIn i_mic_f_r           (PTD6);     //Interrupt Comp Micro F R
+InterruptIn i_mic_r_c           (PTD5);     //Interrupt Comp Micro R C
+
+
+// ********************************************************************
+//ENTRADAS ANALOGICAS
+AnalogIn    a_enc_rig           (PTC2);     //Encoder Left Output_AS_MR
+AnalogIn    a_enc_lef           (PTC1);     //Encoder Right Output_AS_ML
+AnalogIn    a_mic_f_l           (PTB0);     //Analog microphone F L
+AnalogIn    a_mic_f_r           (PTB1);     //Analog microphone F R
+AnalogIn    a_mic_r_c           (PTB2);     //Analog microphone R C
+AnalogIn    a_temp_bat          (PTB3);     //Temperature Battery
+
+
+// ********************************************************************
+
+//PWM OR DIGITAL OUTPUT NORMAL
+//DigitalOut    q_led_whi         (PTE29);    //Led white pwm
+DigitalOut    q_led_red_fro     (PTA4);     //Led Red Front
+DigitalOut    q_led_gre_fro     (PTA5);     //Led Green Front
+DigitalOut    q_led_blu_fro     (PTA12);    //Led Blue Front
+DigitalOut    q_led_red_rea     (PTD4);     //Led Red Rear
+DigitalOut    q_led_gre_rea     (PTA1);     //Led Green Rear
+DigitalOut    q_led_blu_rea     (PTA2);     //Led Blue Rear
+
+
+//SAIDAS DIGITAIS (pwm)
+PwmOut      pwm_mot_rig         (PTE20);    //PWM Enable Motor Right
+PwmOut      pwm_mot_lef         (PTE31);    //PWM Enable Motor Left
+PwmOut      pwm_buzzer          (PTE21);    //Buzzer PWM
+PwmOut      pwm_led_whi         (PTE29);    //Led white pwm
+
+// ********************************************************************
+//SAIDAS ANALOGICAS
+AnalogOut   dac_comp_mic        (PTE30);        //Dac_Comparator MIC
+
+
+/* Powers up all the VCNL4020. */
+void init_Infrared()
+{
+    VCNL40x0_Device.SetCurrent (20);     // Set current to 200mA
+}
+
+/**
+ * Selects the wich infrared to comunicate.
+ *
+ * @param ch - Infrared to read (1..5)
+ */
+void tca9548_select_ch(char ch)
+{
+    char ch_f[1];
+    char addr=0xE0;
+
+    if(ch==0)
+        ch_f[0]=1;
+
+    if(ch>=1)
+        ch_f[0]=1<<ch;
+
+    i2c.start();
+    i2c.write(addr,ch_f,1);
+    i2c.stop();
+}
+
+
+/**
+ * Get ADC value of the chosen infrared.
+ *
+ * @param ch - Infrared to read (1..5)
+ *
+ * Note: for the values of ch it reads (0-right, ... ,4-left, 5-back)
+ */
+long int read_Infrared(char ch) // 0-direita 4-esquerda 5-tras
+{
+    tca9548_select_ch(ch);
+    VCNL40x0_Device.ReadProxiOnDemand (&ProxiValue);    // read prox value on demand
+
+    return ProxiValue;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////     MOTOR       ///////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////
+
+// Calculo do Duty tem de ser revisto, o motor aguenta 6 V e o max definido aqui ronda os 4.2 V
+// consultar pag 39 e 95
+
+/**
+ * Sets speed and direction of the left motor.
+ *
+ * @param Dir - Direction of movement, 0 for back, or 1 for fron
+ * @param Speed - Percentage of speed of the motor (1..100)
+ *
+ * Note: Because of differences in the motors they need to be calibrated, test the robot going front and back
+ *  at different speeds and see if it makes a straigth line
+ */
+void leftMotor(short int Dir,short int Speed)
+{
+    float Duty;
+
+    if(Dir==1) {
+        q_pha_mot_lef=0;            //Andar em frente
+        if(Speed>1000)                   //limite de segurança
+            Speed=1000;
+        if(Speed>0) {
+            Duty=Speed*.082 +35;         // 35 = minimo para o motor rodar
+            q_sleep_mot_lef=1;          //Nano Sleep Motor Left
+            pwm_mot_lef.pulsewidth_us(Duty*5);
+        } else {
+            q_sleep_mot_lef=0;
+        }
+    }
+    if(Dir==0) {
+        q_pha_mot_lef=1;            //Andar para tras
+
+        if(Speed>1000)                   //limite de segurança
+            Speed=1000;
+        if(Speed>0) {
+            Duty=Speed*.082 +35;         // 35 = minimo para o motor rodar
+            q_sleep_mot_lef=1;          //Nano Sleep Motor Left
+            pwm_mot_lef.pulsewidth_us(Duty*5);
+        } else {
+            q_sleep_mot_lef=0;
+        }
+    }
+}
+
+
+/**
+ * Sets speed and direction of the right motor.
+ *
+ * @param Dir - Direction of movement, 0 for back, or 1 for fron
+ * @param Speed - Percentage of speed of the motor (1..100)
+ *
+ * Note: Because of differences in the motors they need to be calibrated, test the robot going front and back
+ *  at different speeds and see if it makes a straigth line
+ */
+void rightMotor(short int Dir,short int Speed)
+{
+    float Duty;
+
+    if(Dir==1) {
+        q_pha_mot_rig=0;            //Andar em frente
+
+        if(Speed>1000)                   //limite de segurança
+            Speed=1000;
+        if(Speed>0) {
+            Duty=Speed*.082 +35;         // 35 = minimo para o motor rodar
+            q_sleep_mot_rig=1;          //Nano Sleep Motor Right
+            pwm_mot_rig.pulsewidth_us(Duty*5);
+        } else {
+            q_sleep_mot_rig=0;
+        }
+    }
+    if(Dir==0) {
+        q_pha_mot_rig=1;            //Andar para tras
+
+
+        if(Speed>1000)                   //limite de segurança
+            Speed=1000;
+        if(Speed>0) {
+            Duty=Speed*.082 +35;         // 35 = minimo para o motor rodar
+            q_sleep_mot_rig=1;          //Nano Sleep Motor Right
+            pwm_mot_rig.pulsewidth_us(Duty*5);
+        } else {
+            q_sleep_mot_rig=0;
+        }
+    }
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////     ENCODER     ///////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////
+
+/**
+ * Reads Position of left magnetic encoder.
+ *
+ * @return The absolute position of the left wheel encoder (0..4095)
+ */
+long int read_L_encoder()
+{
+
+    char data_r_2[5];
+
+    i2c.start();
+    i2c.write(0x6C);
+    i2c.write(0x0C);
+    i2c.read(0x6D,data_r_2,4,0);
+    i2c.stop();
+
+    short int val1=data_r_2[0];
+    short int val2=data_r_2[1];
+    val1=(val1&0xf)*256;
+    long int final=(val2+val1);
+
+    return  final;
+}
+
+
+/**
+ * Reads Position of right magnetic encoder.
+ *
+ * @return The absolute position of the right wheel encoder (0..4095)
+ */
+long int read_R_encoder()
+{
+
+    char data_r_2[5];
+
+    i2c1.start();
+    i2c1.write(0x6C);
+    i2c1.write(0x0C);
+    i2c1.read(0x6D,data_r_2,4,0);
+    i2c1.stop();
+
+    short int val1=data_r_2[0];
+    short int val2=data_r_2[1];
+    val1=(val1&0xf)*256;
+    long int final=(val2+val1);
+
+    return  final;
+}
+
+
+/**
+ * Calculates and returns the value of the  right "incremental" encoder.
+ *
+ * @return The value of "tics" of the right encoder since it was initialized
+ */
+long int incremental_R_encoder()
+{
+    short int next_R=read_R_encoder(); // Reads curent value of the encoder
+    short int dif=next_R-prev_R;       // Calculates the diference from last reading
+
+    if(dif>3000) {                     // Going back and pass zero
+        total_R=total_R-4096+dif;
+    }
+    if(dif<3000&&dif>0) {              // Going front
+        total_R=total_R+dif;
+    }
+    if(dif<-3000) {                    // Going front and pass zero
+        total_R=total_R+4096+dif;
+    }
+    if(dif>-3000&&dif<0) {             // going back
+        total_R=total_R+dif;
+    }
+    prev_R=next_R;                     // Sets last reading for next iteration
+
+    return  total_R;
+}
+
+
+/**
+ * Calculates and returns the value of the  left "incremental" encoder.
+ *
+ * @return The value of "tics" of the left encoder since it was initialized
+ */
+long int incremental_L_encoder()
+{
+    short int next_L=read_L_encoder(); // Reads curent value of the encoder
+    short int dif=-next_L+prev_L;      // Calculates the diference from last reading
+
+    if(dif>3000) {                     // Going back and pass zero
+        total_L=total_L-4096+dif;
+    }
+    if(dif<3000&&dif>0) {              // Going front
+        total_L=total_L+dif;
+    }
+    if(dif<-3000) {                    // Going front and pass zero
+        total_L=total_L+4096+dif;
+    }
+    if(dif>-3000&&dif<0) {             // going back
+        total_L=total_L+dif;
+    }
+    prev_L=next_L;                     // Sets last reading for next iteration
+
+    return  total_L;
+}
+
+
+/**
+ * Calculate the value of both encoder "incremental" every 10 ms.
+ */
+void timer_event()  //10ms interrupt
+{
+    timeout.attach(&timer_event,0.01);
+    if(flag==0) {
+        incremental_R_encoder();
+        incremental_L_encoder();
+    }
+    Odometria();
+
+    return;
+}
+
+
+/**
+ * Set the initial position for the "incremental" enconder and "starts" them.
+ */
+void initEncoders()
+{
+    prev_R=read_R_encoder();
+    prev_L=read_L_encoder();
+    timeout.attach(&timer_event,0.01);
+}
+
+
+/**
+ * Returns to the user the value of the right "incremental" encoder.
+ *
+ * @return The value of "tics" of the right encoder since it was initialized
+ */
+long int R_encoder()
+{
+    wait(0.0001);
+
+    return total_R;
+}
+
+/**
+ * Returns to the user the value of the right "incremental" encoder.
+ *
+ * @return The value of "tics" of the right encoder since it was initialized
+ */
+long int L_encoder()
+{
+    wait(0.0001);
+
+    return total_L;
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////     BATTERY     ///////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////
+
+/**
+ * Reads adc of the battery.
+ *
+ * @param addr - Address to read
+ * @return The voltage of the batery
+ */
+long int read16_mcp3424(char addr)
+{
+    char data[4];
+    i2c1.start();
+    i2c1.read(addr,data,3);
+    i2c1.stop();
+
+    return(((data[0]&127)*256)+data[1]);
+}
+
+/**
+ * Reads adc of the battery.
+ *
+ * @param n_bits - Resolution of measure
+ * @param ch - Chose value to read, if voltage or current of solar or batery
+ * @param gain -
+ * @param addr - Address to write to
+ */
+void write_mcp3424(int n_bits, int  ch, int gain, char  addr)  //chanel 1-4    write -> 0xD0
+{
+
+    int chanel_end=(ch-1)<<5; //shift left
+    char n_bits_end=0;
+
+    if(n_bits==12) {
+        n_bits_end=0;
+    } else if(n_bits==14) {
+        n_bits_end=1;
+    } else if(n_bits==16) {
+        n_bits_end=2;
+    } else {
+        n_bits_end=3;
+    }
+    n_bits_end=n_bits_end<<2; //shift left
+
+    char data[1];
+    data[0]= (char)chanel_end | (char)n_bits_end | (char)(gain-1) | 128;
+    i2c1.start();
+    i2c1.write(addr,data,1);
+    i2c1.stop();
+}
+
+
+/**
+ * Reads adc of the battery.
+ *
+ * @return The voltage of the batery
+ */
+float value_of_Batery()
+{
+    float   R1=75000.0;
+    float   R2=39200.0;
+    float   R3=178000.0;
+    float   Gain=1.0;
+    write_mcp3424(16,3,1,0xd8);
+    float cha3_v2=read16_mcp3424(0xd9); //read  voltage
+    float Vin_v_battery=(((cha3_v2*2.048)/32767))/Gain;
+    float Vin_b_v_battery=(-((-Vin_v_battery)*(R1*R2 + R1*R3 + R2*R3))/(R1*R2));
+    Vin_b_v_battery=(Vin_b_v_battery-0.0)*1.00268;
+
+    return Vin_b_v_battery;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////     RF COMUNICATION     ///////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////
+
+
+/**
+ * Initializes the NRF24 module for comunication.
+ *
+ * Note: if the module is broken or badly connected this init will cause the code to stop,
+ *  if all these messages don't appear thats the case
+ */
+void config_init_nrf()
+{
+    my_nrf24l01p.powerUp(); // powers module
+    my_nrf24l01p.setRfFrequency (2400); // channel 0 (2400-0 ... 2516-116)
+    my_nrf24l01p.setTransferSize(10);   // number of bytes to be transfer
+    my_nrf24l01p.setCrcWidth(8);
+    my_nrf24l01p.enableAutoAcknowledge(NRF24L01P_PIPE_P0); // pipe where data transfer occurs (0..6)
+    pc.printf( "nRF24L01+ Frequency    : %d MHz\r\n",  my_nrf24l01p.getRfFrequency() );
+    pc.printf( "nRF24L01+ Data Rate    : %d kbps\r\n", my_nrf24l01p.getAirDataRate() );
+    pc.printf( "nRF24L01+ TX Address   : 0x%010llX\r\n", my_nrf24l01p.getTxAddress() );
+    pc.printf( "nRF24L01+ RX Address   : 0x%010llX\r\n", my_nrf24l01p.getRxAddress() );
+    pc.printf( "nRF24L01+ CrC Width    : %d CrC\r\n", my_nrf24l01p.getCrcWidth() );
+    pc.printf( "nRF24L01+ TransferSize : %d Paket Size\r\n", my_nrf24l01p.getTransferSize () );
+    my_nrf24l01p.setReceiveMode();
+    my_nrf24l01p.enable();
+    pc.printf( "Setup complete, Starting While loop\r\n");
+}
+
+
+/**
+ * Receives a number from the Arduino.
+ *
+ * @return The value send by the arduino
+ */
+double receiveValue(void)
+{
+    char temp[4];
+    double Val;
+    bool ok=0;
+    my_nrf24l01p.setTransferSize(4);
+    my_nrf24l01p.setReceiveMode();
+    my_nrf24l01p.enable();
+    do {
+        if(my_nrf24l01p.readable(NRF24L01P_PIPE_P0)) {
+            ok = my_nrf24l01p.read( NRF24L01P_PIPE_P0,temp, 4);
+        }
+    } while(ok==0);
+
+    //transformation of temp to convert to original value
+    if(temp[0]==0) // if first elemente is 0 then its negative
+        Val = double(-(int(temp[1])+int(temp[2])*255+int(temp[3])*255*255));
+    else // else its positive
+        Val = double(int(temp[1])+int(temp[2])*255+int(temp[3])*255*255);
+
+    return Val;
+}
+
+
+/**
+ * Sends a number to the Arduino
+ *
+ * @param Value - number to be sent to the Arduino
+ */
+void sendValue(long int Value)
+{
+    bool ok=0;  // comunication sucess, o if failed 1 if sucessfull
+    // double math=Value/65025; // temporary variable save results
+    int zero=1;  // 1 byte, ( - ) if 0 ( + ) if 1
+    int one=0;   // 2 byte (0..255), multiplied by 1
+    int two=0;   // 3 byte (0..255), multiplied by 255
+    int three=0; // 4 byte (0..255), multiplied by 255*255
+
+//transformation of Value to send correctly through pipe
+    if (Value<0) {
+        zero=0;
+        Value=abs(Value);
+    }
+    //  Value=abs(Value);
+
+    double math=Value/65025; // temporary variable save results
+
+    if(math<1) {
+        math=Value/255;
+        if(math<1) {
+            two=0;
+            one=Value;
+        } else {
+            two=(int)math;
+            one=Value % 255;
+        }
+    } else {
+        three=(int)math;
+        math=Value/255;
+        if(math<1) {
+            two=0;
+            one=Value;
+        } else {
+            two=(int)math;
+            one=Value % 255;
+        }
+    }
+    char temp[4]= {(int)zero,(int)one,(int)two,(int)three};
+
+    // Apagar depois de testar mais vezes
+    // pc.printf("1 inidice...%i...\r", temp[0]);
+    // pc.printf("2 inidice...%i...\r", temp[1]);
+    // pc.printf("3 inidice...%i...\r", temp[2]);
+    // pc.printf("4 inidice...%i...\r", temp[3]);
+
+    my_nrf24l01p.setTransferSize(4);
+    my_nrf24l01p.setTransmitMode();
+    my_nrf24l01p.enable();
+    do {
+        ok = my_nrf24l01p.write( NRF24L01P_PIPE_P0,temp, 4);
+        if(ok==1)
+            pc.printf("Done.....%i...\r", Value);
+        else {
+            pc.printf("Failed\r");
+            wait(1);
+        }
+    } while(ok==0);
+}
+
+/**
+ *  Sends matrix to arduino.
+ *
+ * @param matrix - Matrix of numbers to send [0..255]
+ * @param row - Number of rows
+ * @param column - Number of columns
+ */
+void sendMatrix(int (*matrix)[18],int row , int column)
+{
+    short ok=0;
+    short int i =0;
+    short int j=0;
+    short int byte=0;
+    int members=column*row;
+    char message[32]= {0};
+    pc.printf("J ...%d... \r",members);
+
+    my_nrf24l01p.setTransferSize(32);
+    my_nrf24l01p.setTransmitMode();
+    my_nrf24l01p.enable();
+
+    do {
+        int* point = matrix[j];
+
+        do {
+            message[byte]= point[i];
+            if (byte==31 || (i+1)*(j+1)==members) {
+
+                do {
+                    ok = my_nrf24l01p.write( NRF24L01P_PIPE_P0,message, 32);
+                    if(ok==0)
+                        wait(1);
+
+                } while(ok==0);
+
+                byte=-1;
+            }
+
+            byte++;
+            i++;
+
+        } while(i<column);
+
+        i=0;
+        j++;
+    } while(j<row);
+
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////      Sonar     ////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////
+//      Commands of operation with ultrasonic module
+
+//    WRITE OPTION:
+//        ENABLE DC DC CONVERTER          - 0x0C;
+//        DISABLE DC DC CONVERTER         - 0x0B;
+//        START MEASURE LEFT SENSOR       - 0x0A;
+//        START MEASURE FRONT SENSOR      - 0x09;
+//        START MEASURE RIGHT SENSOR      - 0x08;
+//        SENSORS ALWAYS MEASURE ON       - 0x07;
+//        SENSORS ALWAYS MEASURE OFF      - 0x06;
+
+// READ OPTION:
+//        GET MEASURE OF LEFT SENSOR          - 0x05;
+//        GET MEASURE OF FRONT SENSOR         - 0x04;
+//        GET MEASURE OF IGHT SENSOR          - 0x03;
+//        GET STATUS SENSORS ALWAYS MEASURE   - 0x02;
+//        GET STATUS DC DC CONVERTER          - 0x01;
+
+void enable_dc_dc_boost()
+{
+    char data[1];
+    data[0]= 0x0C;
+    wait_ms(1);
+    i2c1.start();
+    i2c1.write(0x30,data,1);
+    i2c1.stop();
+    i2c1.start();
+    i2c1.write(0x30,data,1);
+    i2c1.stop();
+}
+
+
+void disable_dc_dc_boost()
+{
+    char data[1];
+    data[0]= 0x0B;
+    wait_ms(1);
+    i2c1.start();
+    i2c1.write(0x30,data,1);
+    i2c1.stop();
+}
+
+
+void start_read_left_sensor()
+{
+    char data[1];
+    data[0]= 0x0A;
+    wait_ms(1);
+    i2c1.start();
+    i2c1.write(0x30,data,1);
+    i2c1.stop();
+}
+
+
+void start_read_front_sensor()
+{
+    char data[1];
+    data[0]= 0x09;
+    wait_ms(1);
+    i2c1.start();
+    i2c1.write(0x30,data,1);
+    i2c1.stop();
+}
+
+
+void start_read_right_sensor()
+{
+    char data[1];
+    data[0]= 0x08;
+    wait_ms(1);
+    i2c1.start();
+    i2c1.write(0x30,data,1);
+    i2c1.stop();
+}
+
+
+void measure_always_on()  // left, front, right
+{
+    char data[1];
+    data[0]= 0x07;
+    wait_ms(1);
+    i2c1.start();
+    i2c1.write(0x30,data,1);
+    i2c1.stop();
+}
+
+
+void measure_always_off()
+{
+    char data[1];
+    data[0]= 0x06;
+    wait_ms(1);
+    i2c1.start();
+    i2c1.write(0x30,data,1);
+    i2c1.stop();
+}
+
+/**
+ * Returns left sensor value
+ */
+static unsigned int get_distance_left_sensor()
+{
+
+    static char data_r[3];
+    static unsigned int aux;
+    flag=1;
+
+    data_r[0]= 0x05;
+    wait_ms(1);
+    i2c1.start();
+    i2c1.write(0x30,data_r,1);
+    i2c1.stop();
+    wait_ms(10);
+    i2c1.start();
+    i2c1.read(0x31,data_r,2,0);
+    i2c1.stop();
+
+    aux=(data_r[0]*256)+data_r[1];
+    flag=0;
+    return aux;
+    // sensor_left=aux;
+    // pc.printf("\nDistance Left Sensor: %u mm",aux); //0 - 2500mm
+
+}
+
+
+/**
+ * Returns front sensor value
+ */
+static unsigned int get_distance_front_sensor()
+{
+
+    static char data_r[3];
+    static unsigned int aux;
+    flag=1;
+    data_r[0]= 0x04;
+    wait_ms(1);
+    i2c1.start();
+    i2c1.write(0x30,data_r,1);
+    i2c1.stop();
+    wait_ms(10);
+    i2c1.start();
+    i2c1.read(0x31,data_r,2,0);
+    i2c1.stop();
+
+    aux=(data_r[0]*256)+data_r[1];
+    flag=0;
+    return aux;
+    // sensor_front=aux;
+    // pc.printf("\nDistance Front Sensor: %u mm",aux); //0 - 2500mm
+
+}
+
+
+/**
+ * Returns right sensor value
+ */
+static unsigned int get_distance_right_sensor()
+{
+
+    static char data_r[3];
+    static unsigned int aux;
+    flag=1;
+
+    data_r[0]= 0x03;
+    wait_ms(1);
+    i2c1.start();
+    i2c1.write(0x30,data_r,1);
+    i2c1.stop();
+    wait_ms(10);
+    i2c1.start();
+    i2c1.read(0x31,data_r,2,0);
+    i2c1.stop();
+
+    aux=(data_r[0]*256)+data_r[1];
+    flag=0;
+    return aux;
+    // sensor_right=aux;
+    // pc.printf("\nDistance Right Sensor: %u \r",aux); //0 - 2500mm
+
+}
+
+
+void get_status_always_measure()
+{
+
+    static char data_r[3];
+    static unsigned int aux;
+
+    data_r[0]= 0x02;
+    wait_ms(1);
+    i2c1.start();
+    i2c1.write(0x30,data_r,1);
+    i2c1.stop();
+    wait_ms(10);
+    i2c1.start();
+    i2c1.read(0x31,data_r,2,0);
+    i2c1.stop();
+
+    aux=data_r[0];
+    pc.printf("\nStatus of read always on/off: %u ",aux); //0 (off) - 1 (on)
+
+}
+
+
+void get_status_dcdc_converter()
+{
+
+    static char data_r[3];
+    static unsigned int aux;
+
+    data_r[0]= 0x01;
+    wait_ms(1);
+    i2c1.start();
+    i2c1.write(0x30,data_r,1);
+    i2c1.stop();
+    wait_ms(10);
+    i2c1.start();
+    i2c1.read(0x31,data_r,2,0);
+    i2c1.stop();
+
+    aux=data_r[0];
+    pc.printf("\nStatus of DC/DC Converter: %u ",aux); //0 (off) - 1 (on)
+
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////      MISC.      ////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////
+
+
+/**
+ * Initializes the necessary robot pins
+ */
+void init_robot_pins()
+{
+
+    //SAIDAS DIGITAIS (normal)
+    //q_pha_mot_rig=0;            //Phase Motor Right
+    //q_sleep_mot_rig=0;          //Nano Sleep Motor Right
+    //q_pha_mot_lef=0;            //Phase Motor Left
+    //q_sleep_mot_lef=0;          //Nano Sleep Motor Left
+    //q_pow_ena_i2c_p=0;          //Power Enable i2c FET P
+    //q_pow_ena_mic_p=0;          //Power enable Micro FET P
+    //q_pow_as5600_n=1;           //AS5600 Power MOSFET N
+    //q_pow_as5600_p=0;           //AS5600 Power MOSFET P
+    //q_pow_spi=0;                //SPI Power MOSFET P
+    //q_ena_mppt=0;               //Enable MPPT Control
+    //q_boost_ps=1;               //Boost Power Save
+    //q_tca9548_reset=1;          //Reset TCA9548
+
+    //SAIDAS DIGITAIS (normal)
+    q_pha_mot_rig=0;            //Phase Motor Right
+    q_sleep_mot_rig=0;          //Nano Sleep Motor Right
+    q_pha_mot_lef=0;            //Phase Motor Left
+    q_sleep_mot_lef=0;          //Nano Sleep Motor Left
+
+    q_pow_ena_i2c_p=0;         //Power Enable i2c FET P
+    q_pow_ena_mic_p=0;          //Power enable Micro FET P
+    q_pow_as5600_p=0;           //AS5600 Power MOSFET P
+    // q_pow_spi=0;                //SPI Power MOSFET P
+    q_pow_as5600_n=1;           //AS5600 Power MOSFET N
+
+
+    q_ena_mppt=0;               //Enable MPPT Control
+    q_boost_ps=1;               //Boost Power Save
+    q_tca9548_reset=1;          //Reset TCA9548
+
+    //Leds caso seja saida digital:
+    q_led_red_fro=1;          //Led Red Front (led off)
+    q_led_gre_fro=1;          //Led Green Front (led off)
+    q_led_blu_fro=1;          //Led Blue Front (led off)
+    q_led_red_rea=1;          //Led Red Rear (led off)
+    q_led_gre_rea=1;          //Led Green Rear (led off)
+    q_led_blu_rea=1;          //Led Blue Rear (led off)r
+
+
+//********************************************************************
+    //SAIDAS DIGITAIS (pwm)
+    //PWM Enable Motor Right
+    pwm_mot_rig.period_us(500);
+    pwm_mot_rig.pulsewidth_us(0);
+
+    //PWM Enable Motor Left
+    pwm_mot_lef.period_us(500);
+    pwm_mot_lef.pulsewidth_us(0);
+
+    //Buzzer PWM
+    pwm_buzzer.period_us(500);
+    pwm_buzzer.pulsewidth_us(0);
+
+    //LED white
+    pwm_led_whi.period_us(500);
+    pwm_led_whi.pulsewidth_us(0);
+
+}
+
+
+/**
+ * Initializes all the pins and all the modules necessary
+ */
+void initRobot(void)
+{
+    init_robot_pins();
+    enable_dc_dc_boost();
+    init_Infrared();
+    initEncoders();
+    config_init_nrf();
+    enable_dc_dc_boost();
+    wait_ms(100); //wait for read wait(>=150ms);
+    measure_always_on();
+    float value = value_of_Batery();
+    pc.printf("Initialization Successful \n\r");
+    pc.printf("Battery level: %f \n\r",value);
+    if(value < 3.0) {
+        pc.printf(" WARNING: BATTERY NEEDS CHARGING ");
+    }
+
+    // float level = value_of_Batery();
+    // sendValue(int(level*100));
+
+}
+
+
+////////////////////////////////////////////////////
+
+/**
+ * Updates the position and orientation of the robot based on the data from the encoders
+ *
+ * Note: Needs to be calibrated for each robot, in this case the radius of the whells is 3.55
+ * and the distance between them is 7.4
+ */
+void Odometria()
+{
+    long int ticks1d=R_encoder();
+    long int ticks1e=L_encoder();
+
+    long int D_ticks=ticks1d - ticks2d;
+    long int E_ticks=ticks1e - ticks2e;
+
+    ticks2d=ticks1d;
+    ticks2e=ticks1e;
+
+    float D_cm= (float)D_ticks*((3.25*3.1415)/4096);
+    float L_cm= (float)E_ticks*((3.25*3.1415)/4096);
+
+    float CM=(D_cm + L_cm)/2;
+
+    theta +=(D_cm - L_cm)/7.18;
+
+    theta = atan2(sin(theta), cos(theta));
+
+    // meter entre 0
+
+    X += CM*cos(theta);
+    Y += CM*sin(theta);
+
+}
\ No newline at end of file