ISR UC
Energy harvesting mobile robot. Developed at Institute of Systems and Robotics — University of Coimbra.
Dependents: Mapping VirtualForces_debug OneFileToRuleThemAll VirtualForces_with_class ... more
Revision 0:15a30802e719, committed 2017-02-02
- Comitter:
- ISR
- Date:
- Thu Feb 02 12:21:11 2017 +0000
- Child:
- 1:8569ac717e68
- Commit message:
- Initial commit.
Changed in this revision
--- /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