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 1:8569ac717e68, committed 2018-02-20
- Comitter:
- ISR
- Date:
- Tue Feb 20 13:37:47 2018 +0000
- Parent:
- 0:15a30802e719
- Child:
- 2:0435d1171673
- Commit message:
- Encoders working inside thread.
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Encoder.cpp Tue Feb 20 13:37:47 2018 +0000
@@ -0,0 +1,68 @@
+#include "Encoder.h"
+#include "mbed.h"
+
+/** Create Encoder instance, initializing it. */
+Encoder::Encoder(I2C* i2c_in, Mutex* mutex_in, char invert_in) :
+ _i2c(i2c_in),
+ _mutex(mutex_in)
+{
+ prev_L = readAbsolute();
+ total_L = 0;
+ _invert = invert_in;
+};
+
+/** Read encoder raw data (absolute value) */
+long int Encoder::readAbsolute()
+{
+ int addr = 0x6C;
+ char send[5];
+ char receive[5];
+
+ send[0] = 0x0E;
+
+ _mutex->lock();
+ _i2c->write(addr, send, 1);
+ _i2c->read(addr, receive, 2);
+ _mutex->unlock();
+
+ short int val1 = receive[0];
+ short int val2 = receive[1];
+ val1 = (val1 & 0xf)*256;
+ long int result = (val2 + val1);
+ return result;
+}
+
+long int Encoder::incremental()
+{
+ short int next_L;
+ short int dif;
+
+ next_L = readAbsolute(); // Reads curent value of the encoder
+
+ // !!! verificar: para ser correcto deve-se escrever a inversão ao contrário e trocar tb nos encoders.
+ if(_invert == 1)
+ dif = next_L-prev_L;
+ else
+ 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;
+}
+
+long int Encoder::readIncrementalValue()
+{
+ return total_L;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Encoder.h Tue Feb 20 13:37:47 2018 +0000
@@ -0,0 +1,26 @@
+#ifndef ENCODER_H
+#define ENCODER_H
+
+#include "mbed.h"
+
+/** Encoder class.
+ * Manages one magnetic encoder AS5600. Reads its absolute value and performs
+ * the conversion of the value to incremental valeu.
+ */
+class Encoder
+{
+public:
+ Encoder(I2C* i2c_in, Mutex* mutex_in, char invert_in);
+ long int readAbsolute();
+ long int incremental();
+ long int readIncrementalValue();
+
+private:
+ I2C* _i2c;
+ Mutex* _mutex;
+ short int prev_L;
+ long int total_L;
+ char _invert;
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/RF24.lib Tue Feb 20 13:37:47 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/akashvibhute/code/RF24/#ef74df512fed
--- a/nRF24L01P.cpp Thu Feb 02 12:21:11 2017 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,1030 +0,0 @@
-/**
- * @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;
-
-}
-
--- a/nRF24L01P.h Thu Feb 02 12:21:11 2017 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,348 +0,0 @@
-/**
- * @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__ */
-
--- a/robot.h Thu Feb 02 12:21:11 2017 +0000
+++ b/robot.h Tue Feb 20 13:37:47 2018 +0000
@@ -1,54 +1,79 @@
/** @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"
+#include <RF24.h>
+#include "Encoder.h"
+
+I2C i2c(PTC9,PTC8);
+I2C i2c1(PTC11,PTC10);
+
+
+ float X=20;
+ float Y=20;
+
+ Mutex mutex_i2c0, mutex_i2c1;
+ Encoder esquerdo(&i2c, &mutex_i2c0, 0);
+ Encoder direito(&i2c1, &mutex_i2c1, 1);
+
+ Thread thread;
+
+
+
+void odometry_thread()
+{
+ float theta=0;
+ long int ticks2d=0;
+ long int ticks2e=0;
+
+
-void Odometria();
+ while (true) {
+ esquerdo.incremental();
+ direito.incremental();
+ //-------------------------------------------
+ long int ticks1d=direito.readIncrementalValue();
+ long int ticks1e=esquerdo.readIncrementalValue();
+
+ 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);
+ //-------------------------------------------
+
+ Thread::wait(10);
+ }
+}
+
// classes adicionais
-nRF24L01P my_nrf24l01p(PTD2, PTD3, PTD1, PTC13, PTC12, PTA13);
+RF24 radio(PTD2, PTD3, PTD1, PTC12 ,PTC13);
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
@@ -121,11 +146,7 @@
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.
@@ -143,25 +164,42 @@
if(ch>=1)
ch_f[0]=1<<ch;
- i2c.start();
+mutex_i2c0.lock();
i2c.write(addr,ch_f,1);
- i2c.stop();
+ mutex_i2c0.unlock();
+}
+
+
+
+
+
+/* Powers up all the VCNL4020. */
+void init_Infrared()
+{
+
+ for (int i=0; i<6;i++)
+ {
+ tca9548_select_ch(i);
+ VCNL40x0_Device.SetCurrent (20); // Set current to 200mA
+ }
+ tca9548_select_ch(0);
}
/**
- * Get ADC value of the chosen infrared.
+ * Get the distance from of the chosen infrared.
*
- * @param ch - Infrared to read (1..5)
+ * @param ch - Infrared to read (0,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
+float 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;
+ float aux =floor(pow((float)ProxiValue/15104.0,-0.57)*10.0)/10.0 ;
+ return aux;
+ //return ProxiValue;
}
///////////////////////////////////////////////////////////////////////////////////////////////
@@ -256,168 +294,6 @@
///////////////////////////////////////////////////////////////////////////////////////////////
-/////////////////////////////////// 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 ///////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
@@ -425,15 +301,14 @@
* Reads adc of the battery.
*
* @param addr - Address to read
- * @return The voltage of the batery
+ * @return The voltage of the baterry
*/
long int read16_mcp3424(char addr)
{
char data[4];
- i2c1.start();
+ mutex_i2c1.lock();
i2c1.read(addr,data,3);
- i2c1.stop();
-
+ mutex_i2c1.unlock();
return(((data[0]&127)*256)+data[1]);
}
@@ -464,9 +339,9 @@
char data[1];
data[0]= (char)chanel_end | (char)n_bits_end | (char)(gain-1) | 128;
- i2c1.start();
+ mutex_i2c1.lock();
i2c1.write(addr,data,1);
- i2c1.stop();
+ mutex_i2c1.unlock();
}
@@ -491,179 +366,6 @@
}
///////////////////////////////////////////////////////////////////////////////////////////////
-/////////////////////////////// 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
@@ -688,57 +390,9 @@
{
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();
+ mutex_i2c1.lock();
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();
+ mutex_i2c1.unlock();
}
@@ -746,49 +400,30 @@
{
char data[1];
data[0]= 0x07;
- wait_ms(1);
- i2c1.start();
+ mutex_i2c1.lock();
i2c1.write(0x30,data,1);
- i2c1.stop();
+ mutex_i2c1.unlock();
}
-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();
+ mutex_i2c1.lock();
i2c1.write(0x30,data_r,1);
- i2c1.stop();
- wait_ms(10);
- i2c1.start();
- i2c1.read(0x31,data_r,2,0);
- i2c1.stop();
+ i2c1.read(0x31,data_r,2);
+ mutex_i2c1.unlock();
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
-
}
@@ -800,22 +435,17 @@
static char data_r[3];
static unsigned int aux;
- flag=1;
+
data_r[0]= 0x04;
- wait_ms(1);
- i2c1.start();
+
+ mutex_i2c1.lock();
i2c1.write(0x30,data_r,1);
- i2c1.stop();
- wait_ms(10);
- i2c1.start();
- i2c1.read(0x31,data_r,2,0);
- i2c1.stop();
+ i2c1.read(0x31,data_r,2);
+ mutex_i2c1.unlock();
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
}
@@ -828,69 +458,359 @@
static char data_r[3];
static unsigned int aux;
- flag=1;
data_r[0]= 0x03;
- wait_ms(1);
- i2c1.start();
+
+ mutex_i2c1.lock();
i2c1.write(0x30,data_r,1);
- i2c1.stop();
- wait_ms(10);
- i2c1.start();
i2c1.read(0x31,data_r,2,0);
- i2c1.stop();
+ mutex_i2c1.unlock();
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()
-{
+///////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////// RF COMMUNICATION ////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////
- static char data_r[3];
- static unsigned int aux;
+/**
+ * Initializes nrf24l01 module, return value 1 if module is working correcty and 0 if it's not.
+ *
+ ** @param channel - Which RF channel to comunicate on, 0-125
+ *
+ * \warning Channel on Robot and Board has to be the same.
+ */
+ void init_nRF(int channel){
+ int result;
+ result = radio.begin();
+
+ pc.printf( "Initialation nrf24l01=%d\r\n", result ); // 1-working,0-not working
+ radio.setDataRate(RF24_1MBPS);
+ radio.setCRCLength(RF24_CRC_16);
+ radio.setPayloadSize(32);
+ radio.setChannel(channel);
+ radio.setAutoAck(true);
+
+ radio.openWritingPipe(0x314e6f6465);
+ radio.openReadingPipe(1,0x324e6f6465);
+
+ radio.startListening();
+}
+
+char txData, rxData;
+char q[1];
+
- 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();
+/**
+ * Wireless control of motors using nrf24l01 module and FRDM-KL25Z board(robot is working as receiver).
+ */
+void robot_RC()
+{
+ if (radio.available() ) {
- aux=data_r[0];
- pc.printf("\nStatus of read always on/off: %u ",aux); //0 (off) - 1 (on)
-
+ radio.read( &rxData, sizeof(rxData) );
+ pc.putc( rxData );
+
+ q[0]=pc.putc( rxData );
+ if (q[0]=='w'){
+ leftMotor(1,500);
+ rightMotor(1,500);
+ }
+ else if( q[0]=='s'){
+ leftMotor(0,500);
+ rightMotor(0,500);
+ }
+ else if( q[0]=='a'){
+ leftMotor(0,500);
+ rightMotor(1,500);
+ }
+ else if( q[0]=='d'){
+ leftMotor(1,500);
+ rightMotor(0,1000);
+ }
+ else if( q[0]==' '){
+ leftMotor(1,0);
+ rightMotor(1,0);
+ }
+ }
}
-void get_status_dcdc_converter()
-{
+
+/**
+ * Robot is sending values of all sensors (right/left incremental encoder, ultrasonic sensors, infarated sensors and Odometria) to FRDM-KL25Z board using nrf24l01 module.
+ *
+ *Note: Check if module is initilized correctly.
+ */
+void send_Robot_values(){
+
+// WRITE ID:
+// INCREMENTAL LEFT ENCODER - a
+// INCREMENTAL RIGHT ENCODER - b
+// LEFT ULTRASENSOR - c
+// FRONT ULTRASENSOR - d
+// RIGHT ULTRASENSOR - e
+// INFRARED SENSOR 0 - f
+// INFRARED SENSOR 1 - g
+// INFRARED SENSOR 2 - h
+// INFRARED SENSOR 3 - i
+// INFRARED SENSOR 4 - j
+// INFRARED SENSOR 5 - k
+// ODOMETRIA "X" - l
+// ODOMETRIA "Y" - m
+
+// LEFT ENCODER addr: a
+
+ int left_encoder = 0; //read value from device
+ char left_encoder_R[10]; //char array ..._R to read value
+ char left_encoder_W[12]; //char array ..._W to write value
+
+ snprintf(left_encoder_R, 10, "%d", left_encoder); //int value to char array
+
+ for (int i=0; i < 10;i++) //changing posiision of chars in array
+ {
+ left_encoder_W[i+2]=left_encoder_R[i];
+ }
+
+ left_encoder_W[0]='Q'; //adding id to message
+ left_encoder_W[1]='a'; //adding id to parameter
+
+// RIGHT ENCODER addr: b
+
+ int right_encoder = 0; //read value from device
+ char right_encoder_R[10]; //char array ..._R to read value
+ char right_encoder_W[11]; //char array ..._W to write value
+
+ snprintf(right_encoder_R, 10, "%d", right_encoder); //int value to char array
+
+ for (int i=0; i <10;i++) //changing positision of chars in array
+ {
+ right_encoder_W[i+1]=right_encoder_R[i];
+ }
+
+ right_encoder_W[0]='b'; //adding id to parameter
+
+
+// LEFT ULTRASENSOR addr: c
+
+ int left_sensor = get_distance_left_sensor(); //read value from device
+ char left_sensor_R[5]; //char array ..._R to read value
+ char left_sensor_W[6]; //char array ..._W to write value
+
+ snprintf(left_sensor_R, 5, "%d", left_sensor); //int value to char array
- static char data_r[3];
- static unsigned int aux;
+ for (int i=0; i < 5;i++) //changing positision of chars in array
+ {
+ left_sensor_W[i+1]=left_sensor_R[i];
+ }
+
+ left_sensor_W[0]='c'; //adding id to parameter
+
+
+// FRONT ULTRASENSOR addr: d
+
+ int front_sensor = get_distance_front_sensor(); //read value from device
+ char front_sensor_R[5]; //char array ..._R to read value
+ char front_sensor_W[6]; //char array ..._W to write value
+
+ snprintf(front_sensor_R, 5, "%d", front_sensor); //int value to char array
+
+ for (int i=0; i < 5;i++) //changing positision of chars in array
+ {
+ front_sensor_W[i+1]=front_sensor_R[i];
+ }
+
+ front_sensor_W[0]='d'; //adding id to parameter
+
+
+// RIGHT ULTRASENSOR addr: e
+
+ int right_sensor = get_distance_right_sensor(); //read value from device
+ char right_sensor_R[5]; //char array ..._R to read value
+ char right_sensor_W[6]; //char array ..._W to write value
+
+ snprintf(right_sensor_R, 5, "%d", right_sensor); //int value to char array
+
+ for (int i=0; i < 5;i++) //changing positision of chars in array
+ {
+ right_sensor_W[i+1]=right_sensor_R[i];
+ }
+
+ right_sensor_W[0]='e'; //adding id to parameter
+
+
+// INFRARED SENSOR 0 addr: f
+
+ float infrared_0 = read_Infrared(0); //read value from device
+
+ char infrared_0_R[5]; //char array ..._R to read value
+ char infrared_0_W[7]; //char array ..._W to write value
+
+ snprintf(infrared_0_R, 5, "%.1f", infrared_0); //int value to char array
+
+ for (int i=0; i < 6;i++) //changing positision of chars in array
+ {
+ infrared_0_W[i+2]=infrared_0_R[i];
+ }
+
+ infrared_0_W[0]='Z'; //adding id to message
+ infrared_0_W[1]='f'; //adding id to parameter
+
+// INFRARED SENSOR 1 addr: g
+
+ float infrared_1 = read_Infrared(1); //read value from device
+
+ char infrared_1_R[sizeof(infrared_1)+1]; //char array ..._R to read value
+ char infrared_1_W[sizeof(infrared_1)+2]; //char array ..._W to write value
+
+ snprintf(infrared_1_R, sizeof(infrared_1_R), "%.1f", infrared_1); //int value to char array
+
+ for (int i=0; i < sizeof(infrared_1)+2;i++) //changing positision of chars in array
+ {
+ infrared_1_W[i+1]=infrared_1_R[i];
+ }
+
+ infrared_1_W[0]='g'; //adding id to parameter
- 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();
+
+// INFRARED SENSOR 2 addr: h
+
+ float infrared_2 = read_Infrared(2); //read value from device
+
+ char infrared_2_R[sizeof(infrared_2)+1]; //char array ..._R to read value
+ char infrared_2_W[sizeof(infrared_2)+2]; //char array ..._W to write value
+
+ snprintf(infrared_2_R, sizeof(infrared_2_R), "%.1f", infrared_2); //int value to char array
+
+ for (int i=0; i < sizeof(infrared_2)+2;i++) //changing positision of chars in array
+ {
+ infrared_2_W[i+1]=infrared_2_R[i];
+ }
+
+ infrared_2_W[0]='h'; //adding id to parameter
+
+
+// INFRARED SENSOR 3 addr: i
+
+ float infrared_3 = read_Infrared(3); //read value from device
+
+ char infrared_3_R[sizeof(infrared_3)+1]; //char array ..._R to read value
+ char infrared_3_W[sizeof(infrared_3)+2]; //char array ..._W to write value
+
+ snprintf(infrared_3_R, sizeof(infrared_3_R), "%.1f", infrared_3); //int value to char array
+
+ for (int i=0; i < sizeof(infrared_3)+2;i++) //changing positision of chars in array
+ {
+ infrared_3_W[i+1]=infrared_3_R[i];
+ }
+
+ infrared_3_W[0]='i'; //adding id to parameter
+
+
+// INFRARED SENSOR 4 addr: j
+
+ float infrared_4 = read_Infrared(4); //read value from device
+
+ char infrared_4_R[sizeof(infrared_4)+1]; //char array ..._R to read value
+ char infrared_4_W[sizeof(infrared_4)+2]; //char array ..._W to write value
+
+ snprintf(infrared_4_R, sizeof(infrared_4_R), "%.1f", infrared_4); //int value to char array
+
+ for (int i=0; i < sizeof(infrared_4)+2;i++) //changing positision of chars in array
+ {
+ infrared_4_W[i+1]=infrared_4_R[i];
+ }
+
+ infrared_4_W[0]='j'; //adding id to parameter
+
+
+// INFRARED SENSOR 5 addr: k
+
+ float infrared_5 = read_Infrared(5); //read value from device
+
+ char infrared_5_R[sizeof(infrared_5)+1]; //char array ..._R to read value
+ char infrared_5_W[sizeof(infrared_5)+2]; //char array ..._W to write value
+
+ snprintf(infrared_5_R, sizeof(infrared_5_R), "%.1f", infrared_5); //int value to char array
- aux=data_r[0];
- pc.printf("\nStatus of DC/DC Converter: %u ",aux); //0 (off) - 1 (on)
+ for (int i=0; i < sizeof(infrared_5)+2;i++) //changing positision of chars in array
+ {
+ infrared_5_W[i+1]=infrared_5_R[i];
+ }
+
+ infrared_5_W[0]='k'; //adding id to parameter
+
+
+// ODEMETRIA X addr: l
+
+ float X_od = X; //read value from device
+
+ char X_od_R[sizeof(X_od)+1]; //char array ..._R to read value
+ char X_od_W[sizeof(X_od)+2]; //char array ..._W to write value
+
+ snprintf(X_od_R, sizeof(X_od_R), "%.1f", X_od); //int value to char array
+
+ for (int i=0; i < sizeof(X_od)+2;i++) //changing positision of chars in array
+ {
+ X_od_W[i+1]=X_od_R[i];
+ }
+
+ X_od_W[0]='l'; //adding id to parameter
+
+
+// ODEMETRIA X addr: m
+
+ float Y_od = Y; //read value from device
+
+ char Y_od_R[sizeof(Y_od)+1]; //char array ..._R to read value
+ char Y_od_W[sizeof(Y_od)+2]; //char array ..._W to write value
+
+ snprintf(Y_od_R, sizeof(Y_od_R), "%.1f", Y_od); //int value to char array
-}
+ for (int i=0; i < sizeof(Y_od)+2;i++) //changing positision of chars in array
+ {
+ Y_od_W[i+1]=Y_od_R[i];
+ }
+
+ Y_od_W[0]='m'; //adding id to parameter
+
+
+
+/////Preparing messages to send//////
+
+char send_Z[30];
+for (int i =0; i<sizeof(left_encoder_W);i++){
+ send_Z[i]=left_encoder_W[i];
+ }
+strcat(send_Z,right_encoder_W);
+strcat(send_Z,left_sensor_W);
+strcat(send_Z,front_sensor_W);
+strcat(send_Z,right_sensor_W);
+strcat(send_Z,X_od_W);
+
+char send_Q[40];
+for (int i =0; i<sizeof(infrared_0_W);i++){
+ send_Q[i]=infrared_0_W[i];
+ }
+strcat(send_Q,infrared_1_W);
+strcat(send_Q,infrared_2_W);
+strcat(send_Q,infrared_3_W);
+strcat(send_Q,infrared_4_W);
+strcat(send_Q,infrared_5_W);
+strcat(send_Q,Y_od_W);
+
+
+
+/////Sending messages//////
+ radio.stopListening();
+ radio.write( &send_Z, sizeof(send_Z));
+ radio.write( &send_Q, sizeof(send_Q) );
+ radio.startListening();
+ }
///////////////////////////////////////////////////////////////////////////////////////////////
@@ -958,70 +878,42 @@
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
+ *
+ * @param channel - Which RF channel to comunicate on, 0-125.
+ *\note If you are not using RF module put random number.
+ * \warning Channel on Robot and Board has to be the same.
*/
-void initRobot(void)
-{
+void initRobot(int channel)
+{
+ pc.printf("Battery level: \n\r");
+ init_nRF(channel);
init_robot_pins();
enable_dc_dc_boost();
- init_Infrared();
- initEncoders();
- config_init_nrf();
+ wait_ms(100); //wait for read wait(>=150ms);
enable_dc_dc_boost();
+ measure_always_on();
wait_ms(100); //wait for read wait(>=150ms);
- measure_always_on();
+
+ init_Infrared();
float value = value_of_Batery();
+
+ thread.start(odometry_thread);
+
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