File content as of revision 14:454090793a35:

#include <stdlib.h>
#include <iostream>
#include <string.h>
#include <mbed.h>
#include "dot_util.h"
#include "RadioEvent.h"
#include "itoa.h"

 
/////////////////////////////////////////////////////////////////////////////
// -------------------- DOT LIBRARY REQUIRED ------------------------------//
// * Because these example programs can be used for both mDot and xDot     //
//     devices, the LoRa stack is not included. The libmDot library should //
//     be imported if building for mDot devices. The libxDot library       //
//     should be imported if building for xDot devices.                    //
// * https://developer.mbed.org/teams/MultiTech/code/libmDot-dev-mbed5/    //
// * https://developer.mbed.org/teams/MultiTech/code/libmDot-mbed5/        //
// * https://developer.mbed.org/teams/MultiTech/code/libxDot-dev-mbed5/    //
// * https://developer.mbed.org/teams/MultiTech/code/libxDot-mbed5/        //
/////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////
// * these options must match between the two devices in   //
//   order for communication to be successful
//-------------------MDOT variables------------------------//
/////////////////////////////////////////////////////////////
static uint8_t network_address[] = { 0x00, 0x11, 0x22, 0x33 };
static uint8_t network_session_key[] = { 0x00, 0x11, 0x22, 0x33, 0x00, 0x11, 0x22, 0x33, 0x00, 0x11, 0x22, 0x33, 0x00, 0x11, 0x22, 0x33 };
static uint8_t data_session_key[] = { 0x33, 0x22, 0x11, 0x00, 0x33, 0x22, 0x11, 0x00, 0x33, 0x22, 0x11, 0x00, 0x33, 0x22, 0x11, 0x00 };

mDot* dot = NULL;
lora::ChannelPlan* plan = NULL;
//--------------End of MDOT variables-------------------//

Serial pc(USBTX, USBRX);

// ADXL372 Slave SPI
DigitalOut MOSI(D11);
DigitalIn MISO(D12);
DigitalOut SPI_CLK(D13);
DigitalOut SPI_CS(D10);
//InterruptIn INT1();
//InterruptIn INT2();


// ADXL372 Slave I2C
I2C ADXL372(I2C_SDA, I2C_SCL);  // (D14,D15) (MISO, CS)

// ADT7410 Temperature
I2C ADT7410(I2C_SDA, I2C_SCL);      // Attempt at making I2C connection to slaves (D14,D15)
InterruptIn ADT7410_Int(D2);    // Allow this pin for ADT7410 Interrupt critical temperature notice

// DS7505s Temperature
I2C DS7505(I2C_SDA, I2C_SCL);      // Attempt at making I2C connection to slaves (D14,D15)

// Create reocurring interrupt function that could be used to periodically take temperatures
// Not working right now due to some mutex initialize error
// Suspect that it is due to it having be a RTOS task thing
// Should probably go back to using an in processor timer interrupt instead of mbed
Ticker interruptEverything;

const int ADT7410_Address_7BIT = 0x49;  // A0 set HIGH and A1 set LOW
const int ADT7410_Address_8BIT = ADT7410_Address_7BIT << 1; // Shift 1 bit to left for R/~W bit, and basic I2C format

const int ADXL372_Address_7bit = 0x1D;      // Address for the I2C if MISO pulled low, 0x53 if pulled high
const int ADXL372_Address_8bit = ADXL372_Address_7bit << 1; // Same

const int DS7505s_Address_7bit = 0x48;  // A0 set LOR, A1 set LOW, A2 set LOW
const int DS7505s_Address_8bit = DS7505s_Address_7bit << 1; // Same


//-------------------All prototype functions-----------------------//
void ADXL372Initialize(void);

int accelerometerI2CWrite(int hexAddress, int hexData);
char * accelerometerI2CRead(int hexAddress);
void ADXL372Reset(void);
void I2CSelfTest(void);
void BitBangSPIWrite(const unsigned char regAddr, const unsigned char regData);
uint8_t BitBangSPIRead (const unsigned char regAddr);

int ADT7410Write(unsigned char registerAddress, unsigned char data);
char * ADT7410Read(int hex);

int DS7505sWrite(unsigned char registerAddress, unsigned char data);
char * DS7505sRead(int hex);

char twosComplementConversion(char value);
void flushSerialBuffer(void);
//---------------------End of prototype functions-----------------------------//

void printMenu(){
    pc.printf("Please eneter a debug option: \n\r"
                "1: I2C Read converted values from Accelerometer ADXL372\n\r"
                "2: Read converted values from Temperature ADT7410\n\r"
                "3: Read raw values from Accelerometer ADXL372\n\r"
                "4: Read raw values from Temperature ADT7410\n\r"
                "5: Initialize Accelerometer with I2C\n\r"
                "6: Reset Accelerometer with I2C\n\r"
                "7: Perform self test with I2C\n\r"
                "8: Send Temperature data\n\r"
                "9: SPI Read values from Accelerometer ADXL372\n\r"
                "a: SPI reset for ADXL372\n\r"
                "b: Initialize Accelerometer with SPI\n\r"
                "c: Check data in status register with SPI\n\r"
                "d: Perform self test with SPI\n\r"
                "e: Perform two's complement on accel data\n\r");
}


/*******************************************************************************
 *  Function to be called by the ticker interrupt
 *  Read temperatures and send it 
 *
 *
 ******************************************************************************/
////////////////////////////////////////////////////////////////////////////////
void interruptReadTemperature(void){
    std::vector<uint8_t> tx_data;
    uint16_t temperatures;
    char data[2] = {0, 0};
    char cmd[1];
    cmd[0] = 0x00;
    //pc.printf("Register Addres is: %x \n\r", cmd[0]); 
    if(ADT7410.write(ADT7410_Address_8BIT, cmd,1) == 0){
        if(ADT7410.read(ADT7410_Address_8BIT, data, 2) == 0){
            temperatures = ((data[0] << 8) | data[1]) >> 3;
            tx_data.push_back((temperatures >> 8) & 0xFF);
            tx_data.push_back(temperatures & 0xFF);
            logInfo("light: %lu [0x%04X]", temperatures, temperatures);
            send_data(tx_data);
            //return (data[0] >> 8 | data[1])>>3;     // Explained here: https://stackoverflow.com/a/141576 SOOO GOOOOODDD
            
        }
    }
}
////////////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////////////
/*                              _     
               ____ ___  ____ _(_)___ 
              / __ `__ \/ __ `/ / __ \
             / / / / / / /_/ / / / / /
            /_/ /_/ /_/\__,_/_/_/ /_/ 
                          
 *//////////////////////////////////////////////////////////////////////////////            
int main() {
    // Custom event handler for automatically displaying RX data
    //interruptEverything.attach(&interruptReadTemperature, 7.0);
    RadioEvent events;
    uint32_t tx_frequency;
    uint8_t tx_datarate;
    uint8_t tx_power;
    uint8_t frequency_band;
    
    // Points to the returned char pointer from called functions
    char * rawTempValues;           // Could change to uint8_t, same for other char pointers
    
    // Save converted values here
    uint16_t convertedTempValue;    // Data values must be uint16_t for conversion and send prep
    char *accelValues;
    uint16_t XData;
    uint16_t YData;
    uint16_t ZData;
    int regAddress;

    // Change baud rate in serial terminal to this value
    pc.baud(115200);
    
    
    mts::MTSLog::setLogLevel(mts::MTSLog::TRACE_LEVEL);
    
    // Sometimes when calling this, it creates error: type specifier expected
    // Even with identical include files I would get this in another workspace.
    plan = new lora::ChannelPlan_US915();

    logInfo("Now asserting");
    assert(plan);

    // Careful when using this. The production ready libmdot-mbed5 has a void constructor
    // Therefore, can only use the libmDot-dev-mbed5 version, for now.
    dot = mDot::getInstance(plan);
    assert(dot);

    logInfo("mbed-os library version: %d", MBED_LIBRARY_VERSION);

    // start from a well-known state
    logInfo("defaulting Dot configuration");
    dot->resetConfig();

    // make sure library logging is turned on
    dot->setLogLevel(mts::MTSLog::INFO_LEVEL);

    // attach the custom events handler
    dot->setEvents(&events);

    // update configuration if necessary
    if (dot->getJoinMode() != mDot::PEER_TO_PEER) {
        logInfo("changing network join mode to PEER_TO_PEER");
        if (dot->setJoinMode(mDot::PEER_TO_PEER) != mDot::MDOT_OK) {
            logError("failed to set network join mode to PEER_TO_PEER");
        }
    }
    
/*
 *  Get the Frequency and then choose transfer frequency, datarate, and power accordingly
 *
 */
////////////////////////////////////////////////////////////////////////////////
    frequency_band = dot->getFrequencyBand();
    switch (frequency_band) {
        case lora::ChannelPlan::EU868_OLD:
        case lora::ChannelPlan::EU868:
            // 250kHz channels achieve higher throughput
            // DR_6 : SF7 @ 250kHz
            // DR_0 - DR_5 (125kHz channels) available but much slower
            tx_frequency = 869850000;
            tx_datarate = lora::DR_6;
            // the 869850000 frequency is 100% duty cycle if the total power is under 7 dBm - tx power 4 + antenna gain 3 = 7
            tx_power = 4;
            break;

        case lora::ChannelPlan::US915_OLD:
        case lora::ChannelPlan::US915:
        case lora::ChannelPlan::AU915_OLD:
        case lora::ChannelPlan::AU915:
            // 500kHz channels achieve highest throughput
            // DR_8 : SF12 @ 500kHz
            // DR_9 : SF11 @ 500kHz
            // DR_10 : SF10 @ 500kHz
            // DR_11 : SF9 @ 500kHz
            // DR_12 : SF8 @ 500kHz
            // DR_13 : SF7 @ 500kHz
            // DR_0 - DR_3 (125kHz channels) available but much slower
            tx_frequency = 915500000;
            tx_datarate = lora::DR_13;
            // 915 bands have no duty cycle restrictions, set tx power to max
            tx_power = 20;
            break;

        case lora::ChannelPlan::AS923:
        case lora::ChannelPlan::AS923_JAPAN:
            // 250kHz channels achieve higher throughput
            // DR_6 : SF7 @ 250kHz
            // DR_0 - DR_5 (125kHz channels) available but much slower
            tx_frequency = 924800000;
            tx_datarate = lora::DR_6;
            tx_power = 16;
            break;

        case lora::ChannelPlan::KR920:
            // DR_5 : SF7 @ 125kHz
            tx_frequency = 922700000;
            tx_datarate = lora::DR_5;
            tx_power = 14;
            break;

        default:
            while (true) {
                logFatal("no known channel plan in use - extra configuration is needed!");
                wait(5);
            }
            break;
    }
    
////////////////////////////////////////////////////////////////////////////////

    // in PEER_TO_PEER mode there is no join request/response transaction
    // as long as both Dots are configured correctly, they should be able to communicate
    update_peer_to_peer_config(network_address, network_session_key, data_session_key, tx_frequency, tx_datarate, tx_power);
    
    // save changes to configuration
    logInfo("saving configuration");
    if (!dot->saveConfig()) {
        logError("failed to save configuration");
    }

    // Display configuration
    // It's gonna output a lot of information onto the Serial Terminal
    display_config();
    
/*
 *
 *  From here on is my own code
 *  Can add more options to choose from
 *
 */
////////////////////////////////////////////////////////////////////////////////
    printMenu();
    flushSerialBuffer();
    pc.printf("\n\rChoose what you want to do: \n\r");
    
    char userInput = pc.getc();
    while(1){
        // Create a vector of uint8_t elements to be sent later
        std::vector<uint8_t> tx_data;
        
        // Checks if a character has been pressed;
        // Works right now for 1 digit numbers :(
        // Change to work with larger inputs
        if(pc.readable())   
        {
            userInput = pc.getc();
            switch(userInput){
            case 49:    // 1
                pc.printf("Reading converted values from accelerometer\n\r");
                for(int i = 0; i < 15; ++i){
                    regAddress = 0x08;  // This is the register address for XData
                    accelValues = accelerometerI2CRead(regAddress);
                    XData = ((*(accelValues + 0) << 8) | *(accelValues + 1)) >> 4;  // Combine two bytes into short int, remove last 4 flag bits
                    YData = ((*(accelValues + 2) << 8) | *(accelValues + 3)) >> 4;
                    ZData = ((*(accelValues + 4) << 8) | *(accelValues + 5)) >> 4;
                    pc.printf("\n %d: X: 0x%x | Y: 0x%x | Z: 0x%x \n\r", i+1, XData, YData, ZData);
                    wait(0.2);
                }
                break;
            case 50:    // 2
                pc.printf("Reading converted values from temperature\n\r");
                for(int i = 0; i < 10; ++i){
                    regAddress = 0x00;
                    rawTempValues = ADT7410Read(regAddress);
                    convertedTempValue = ((*(rawTempValues + 0) << 8) | *(rawTempValues + 1)) >> 3; // Combine the two bytes into 
                                                                                                    // a short int variable(16 bits), remove last 3 bits
                                                                                                    
                    pc.printf("\n %d: Temperature is: 0x%x \n\r", i+1, convertedTempValue);
                }
                
                break;
            case 51:    // 3
                pc.printf("Reading raw values from accelerometer\n\r");
                for(int i = 0; i < 15; ++i){
                    regAddress = 0x08;
                    accelValues = accelerometerI2CRead(regAddress);
                    XData = ((*(accelValues + 0) << 8) | *(accelValues + 1)) >> 4;  // Combine two bytes into short int(16 bits), remove last 4 flag bits
                    YData = ((*(accelValues + 2) << 8) | *(accelValues + 3)) >> 4;
                    ZData = ((*(accelValues + 4) << 8) | *(accelValues + 5)) >> 4;
                    pc.printf("\n %d: X:: H: %x | L: %x | Y:: H: %x | L: %x | Z: H: %x | L: %x \n\r", i+1, *(accelValues + 0), 
                                                                                                           *(accelValues + 1), 
                                                                                                           *(accelValues + 2),
                                                                                                           *(accelValues + 3),
                                                                                                           *(accelValues + 4),
                                                                                                           *(accelValues + 5));
                    wait(0.2);
                }
                break;
            case 52:    // 4
                pc.printf("Reading raw values from temperature\n\r");
                for(int i = 0; i < 10; ++i){
                    regAddress = 0x00;
                    rawTempValues = ADT7410Read(regAddress);
                    pc.printf("\n %d: Temperature is: HIGH BYTE: %x | LOW BYTE: %x \n\r", i+1, *(rawTempValues + 0), 
                                                                                               *(rawTempValues + 1));
                }
                break;
            case 53:    // 5
                ADXL372Initialize();
                break;
            case 54:    // 6
                ADXL372Reset();
                break;
                
            case 55:    // 7: perform self test with I2C
                I2CSelfTest();
                break;
            case 56:    // 8: Read Temperature raws
                regAddress = 0x00;
                rawTempValues = ADT7410Read(regAddress);
                convertedTempValue = ((*(rawTempValues + 0) << 8) | *(rawTempValues + 1)) >> 3; // Combine the two bytes

                tx_data.push_back((convertedTempValue >> 8) & 0xFF);
                tx_data.push_back(convertedTempValue & 0xFF);
                logInfo("light: %lu [0x%04X]", convertedTempValue, convertedTempValue);
                send_data(tx_data);
                break;
                
            case 57:    // 9: Read Accelerometer SPI
                uint8_t MSB;
                uint8_t LSB;
                
                
                for(int i = 0; i < 15; ++i){
                    
                    MSB = BitBangSPIRead(0x08);     // XData MSB
                    LSB = BitBangSPIRead(0x09);     // XData LSB
                    XData = ((MSB << 8) | LSB) >> 4;
                    
                    MSB = BitBangSPIRead(0x0A);     // YData MSB
                    LSB = BitBangSPIRead(0x0B);     // YData LSB
                    YData = ((MSB << 8) | LSB) >> 4;
                    
                    MSB = BitBangSPIRead(0x0C);     // ZData MSB
                    LSB = BitBangSPIRead(0x0D);     // ZData LSB
                    ZData = ((MSB << 8 ) | LSB) >> 4;
                    
                    pc.printf("\n %d: X: 0x%x | Y: 0x%x | Z: 0x%x \n\r", i+1, XData, YData, ZData);
                    wait(0.2);
                }
                break;
            case 97:    // a: Reset Accelerometer with SPI
                int intRegister;
                char input[4];
                char passRegister[1];
                int intData;
                char passData[1];
                BitBangSPIWrite(0x41, 0x52);
                pc.printf("Done \n\r");
                break;
            case 98:    // b: Initialize Accelerometer with SPI
                BitBangSPIWrite(0x24, 0x01);    // Turn on X
                BitBangSPIWrite(0x26, 0x01);    // Turn on Y
                BitBangSPIWrite(0x28, 0x01);    // Turn on Z
                pc.printf("Done\n\r");
                break;
            case 99:    // c: Check status with SPI
                uint8_t statusData = BitBangSPIRead(0x04);
                
                pc.printf("0x%x in status\n\r", statusData);
                break;
            case 100:    // d: Perform self-test
                uint8_t testResult;
                BitBangSPIWrite(0x3F, 0x03);    // put to fullbandwidth measurement mode and lpf enaled(0)
                
                BitBangSPIWrite(0x40, 0x01);    // start self test
                testResult = BitBangSPIRead(0x40);
                while(!(BitBangSPIRead(0x40) & 0x02)){}
                wait(0.4);
                
                testResult = BitBangSPIRead(0x40);
                
                if(testResult & 0x04)
                {
                    pc.printf("It passed \n\r");
                }
                pc.printf("0x%x\n\r", testResult);
                break;
            case 101:   // e for two's complement of accel data
                regAddress = 0x08;
                char Xmsb;
                char Xlsb;
                char Ymsb;
                char Ylsb;
                char Zmsb;
                char Zlsb;
                
                accelValues = accelerometerI2CRead(regAddress);
                
                Xmsb = *(accelValues + 0);
                Xlsb = *(accelValues + 1);
                Ymsb = *(accelValues + 2);
                Ylsb = *(accelValues + 3);
                Zmsb = *(accelValues + 4);
                Zlsb = *(accelValues + 5);
                pc.printf("\n X:: H: %x | L: %x | Y:: H: %x | L: %x | Z: H: %x | L: %x \n\r", *(accelValues + 0),
                                                                                              *(accelValues + 1),
                                                                                              *(accelValues + 2),
                                                                                              *(accelValues + 3),
                                                                                              *(accelValues + 4),
                                                                                              *(accelValues + 5));
                pc.printf("Converted \n\r");
                
                pc.printf("\n X:: H: %x | L: %x | Y:: H: %x | L: %x | Z: H: %x | L: %x \n\r", twosComplementConversion(Xmsb),
                                                                                              twosComplementConversion(Xlsb),
                                                                                              twosComplementConversion(Ymsb),
                                                                                              twosComplementConversion(Ylsb),
                                                                                              twosComplementConversion(Zmsb),
                                                                                              twosComplementConversion(Zlsb));
                wait(0.2);
                
                break;
            default:
                printMenu();
                break;
            }
        }
        
    }
    
    return 0;    
}
////////////////////////////////////////////////////////////////////////////////


/*******************************************************************************
 *                                                                             
 *  I2C function for the the ADXL372 accelerometer for a write sequence
 *  Param:
 *      hexAddress: Pass the hexadecimal value for what register you want
 *      hexData: Pass the hexadecimal value for what data you want to send
 *               i.e. hexadecimal represenatation of certain bits
 *  Returns:             
 *      1: Write was a complete success
 *      2: Writing data to register failed
 *      3: Writing to register Address failed     
 ******************************************************************************/
////////////////////////////////////////////////////////////////////////////////
int accelerometerI2CWrite(int hexAddress, int hexData){
 //--------- One full writing cycle for ADXL372 for X enable ------------------//
/*    '0' - NAK was received
 *    '1' - ACK was received, <---- This good
 *    '2' - timeout
 */
    int flag;
    int registerAddress = hexAddress;
    int data = hexData;
    
    ADXL372.start();
    flag = ADXL372.write(ADXL372_Address_8bit);
    if(flag == 1)
    {
        //pc.printf("Write to I2C address success\n\r");
        wait(0.1);
        flag = ADXL372.write(registerAddress);
        if(flag == 1)
        {
            //pc.printf("Write to register 0x%x address success\n\r", registerAddress);
            flag = ADXL372.write(data);
            if(flag == 1)
            {
                pc.printf("Writing data 0x%x to register address success\n\r", data);
                ADXL372.stop();
                return 1;
            }else {ADXL372.stop(); return 2;} 
        }else {ADXL372.stop(); return 3;}
    }else ADXL372.stop();
       
    return 0;
// ---------------- End of writing cycle --------------------------//   
}
////////////////////////////////////////////////////////////////////////////////


/*******************************************************************************
 *  I2C read sequence for the accelerometer
 *  Param:
 *      hexAddress: pass the hexadecimal representation of desired Register address
 *  Return:
 *      Char pointer to the array of read data. 
 *
 *  Right now it works only for the XData, YData, ZData because I wrote it to read
 *  6 bytes(6 registers).
 *  Should change it to be 1 byte at a time
 ******************************************************************************/
////////////////////////////////////////////////////////////////////////////////
char * accelerometerI2CRead(int hexAddress){
    char accelData[6];
    char registerAddress[1];
    registerAddress[0] = hexAddress;
    
    // Perform mbed's way sending a start bit, then device address[r/~w], and then the register address
    // Also if it succeeds, continue to the next operation
    if(ADXL372.write(ADXL372_Address_8bit, registerAddress, 1) == 0){
        
        // If previous sequence works, get 6 bytes into char array accelData
        // Char array because it uses 1 byte(8bits)
        // Should probably change it to uint8_t type
        if(ADXL372.read(ADXL372_Address_8bit, accelData, 6) == 0){
            return accelData;
        }else pc.printf("Failed to read\n\r");
    }else pc.printf("Failed to write\n\r");
    return 0; // Only if it fails   
}
////////////////////////////////////////////////////////////////////////////////



/*******************************************************************************
 *  Initializes whatever settings you want for the accelerometer
 *  Can change it to use the previous I2C write function instead of all this mess
 *
 ******************************************************************************/  
////////////////////////////////////////////////////////////////////////////////
void ADXL372Initialize(void){
    accelerometerI2CWrite(0x3F, 0x0F);  // Enable I2C highspeed,Low Pass, High pass and full bandwidth measurement mode
/*     accelerometerI2CWrite(0x24, 0x01);  // X used for activity threshold
    accelerometerI2CWrite(0x26, 0x01);  // Y used for activity threshold
    accelerometerI2CWrite(0x28, 0x01);  // Z used for activity threshold  */
}
////////////////////////////////////////////////////////////////////////////////



/*******************************************************************************
 * BitBangSPIWrite for ADXL372 if you wire it up as SPI
 *
 * Sends the 6-0bits of desired register with LSB of transmission bit R/!W
 * 
 ******************************************************************************/
 ///////////////////////////////////////////////////////////////////////////////
 void BitBangSPIWrite(const unsigned char regAddr, const unsigned char regData)
{
    unsigned char SPICount;                                          // Counter used to clock out the data

    unsigned char SPIData;                                          // Define a data structure for the SPI data   
    SPI_CS = 0;                                                     // Make sure we start with active-low CS high
    SPI_CLK = 0;                                                      // and CK low
    
    SPIData = regAddr;                                            // Preload the data to be sent with Address
    SPI_CS = 1;                                                   // Set active-low CS low to start the SPI cycle 
    
    for (SPICount = 0; SPICount < 8; SPICount++)                  // Prepare to clock out the Address byte
    {
    if (SPIData & 0x80)                                         // Check for a 1 at MSB
        MOSI = 1;                                               // and set the MOSI line appropriately
    else
        MOSI = 0;
    SPI_CLK = 1;                                                 // Toggle the clock line
    SPI_CLK = 0;
    SPIData <<= 1;                                                  // Rotate to get the next bit
    }                                                             // and loop back to send the next bit
      
    // Repeat for the Data byte
    SPIData = regData;                                            // Preload the data to be sent with Data
    for (SPICount = 0; SPICount < 8; SPICount++)
    {
    if (SPIData & 0x80)
        MOSI = 1;
    else
        MOSI = 0;
    SPI_CLK = 1;
    SPI_CLK = 0;
    SPIData <<= 1;                                              // After each bit, move next bit one to left
    }          
    SPI_CS = 0;
    MOSI = 0;
}
////////////////////////////////////////////////////////////////////////////////



/*******************************************************************************
 *  BitBangSPIRead for ADXL372 if you wire it up as SPI
 *  
 *  
 *
 ******************************************************************************/
////////////////////////////////////////////////////////////////////////////////
uint8_t BitBangSPIRead (uint8_t regAddr)
{

  unsigned char SPICount;   // Counter used to clock out the data
  
  uint8_t SPIData;                  
  
  SPI_CS = 0;   // Make sure we start with active-low CS high
  SPI_CLK = 0;  // and CK low
  SPIData = regAddr;    // Preload the data to be sent with Address and Data

  SPI_CS = 1;   // Set active-low CS low to start the SPI cycle
  for (SPICount = 0; SPICount < 8; SPICount++)  // Prepare to clock out the Address and Data
  {
    if (SPIData & 0x80)
      MOSI = 1;
    else
      MOSI = 0;
    SPI_CLK = 1;
    SPI_CLK = 0;
    SPIData <<= 1;
  }// and loop back to send the next bit
  
  MOSI = 0; // Reset the MOSI data line
  
  SPIData = 0;
  for (SPICount = 0; SPICount < 8; SPICount++)  // Prepare to clock in the data to be read
  {
    SPIData <<=1;   // Rotate the data, keep previous bit value
    SPI_CLK = 1;    // Raise the clock to clock the data out of the MAX7456
    SPIData += SPI_MISO;    // Read the data one bit at a time, starting from MISO MSB
    SPI_CLK = 0;    // Drop the clock ready for the next bit
  }// and loop back for next bit
  SPI_CS = 0;   // Raise CS
                      
  return ((uint8_t)SPIData);  // Finally return the read data
}

/*******************************************************************************
 *  Not really used at the moment
 *  Not really needed. But keep just in case because I don't want to rewrite it
 ******************************************************************************/
////////////////////////////////////////////////////////////////////////////////
char twosComplementConversion(char value)
{
    /*
     * Go from bit 0 to bit 7 and invert them
     * Then finally add 1
     */
    char mask = value & 0x80;
    if(mask == 0x80){   // Check for sign
        value = ~value + 1;
        return value;
    } 
    return value;
}
////////////////////////////////////////////////////////////////////////////////


/*******************************************************************************
 *  Performs one byte write I2C protocol
 *  PARAM:
 *      registerAddress: register you want access to in device, one byte char hex format
 *      data: one byte data that you want to write to device register
 *  Return:
 *      0: failure at writing i2c address
 *      1: successful write
 *      2: failure at writing data
 *      3: failure at writing register address
 ******************************************************************************/
////////////////////////////////////////////////////////////////////////////////
int ADT7410Write(unsigned char registerAddress, unsigned char data){
    int flag;
    ADT7410.start();
    flag = ADT7410.write(ADT7410_Address_8BIT);
    if(flag == 1)
    {
        pc.printf("Write to I2C address success\n\r");
        wait(0.1);
        flag = ADT7410.write(registerAddress);
        if(flag == 1)
        {
            pc.printf("Write to register 0x%x address success\n\r", registerAddress);
            flag = ADT7410.write(data);
            if(flag == 1)
            {
                pc.printf("Writing data 0x%x to register address success\n\r", data);
                ADT7410.stop();
                return 1;
            }else {ADT7410.stop(); return 2;} 
        }else {ADT7410.stop(); return 3;} 
    }else ADT7410.stop();
       
    return 0;
}
////////////////////////////////////////////////////////////////////////////////

/*******************************************************************************
 *  I2C Read function for ADT7410 Temperature sensor
 *  Param:
 *      hex: hexadecimal representation for desired register
 *  Return:
 *      Char pointer to the array of data values.
 *  Could also change from a char pointer to a uint8_t pointer.
 *
 ******************************************************************************/
////////////////////////////////////////////////////////////////////////////////
char * ADT7410Read(int hex){
    //short int convertedVal;
    char data[2] = {0, 0};
    char cmd[1];
    cmd[0] = hex;
    //pc.printf("Register Addres is: %x \n\r", cmd[0]); 
    if(ADT7410.write(ADT7410_Address_8BIT, cmd,1) == 0){
        if(ADT7410.read(ADT7410_Address_8BIT, data, 2) == 0){
            
            return data;
            //return (data[0] << 8 | data[1])>>3;     // Explained here: https://stackoverflow.com/a/141576 SOOO GREAT
            
        }else {pc.printf("Failed to read \n\r"); return data;}
    }else {pc.printf("Failed to write \n\r"); return data;}
    
}
////////////////////////////////////////////////////////////////////////////////


/*******************************************************************************
 *  ADXL372 reset function
 *  Resets all registers and settings back to default
 *  Basically the same as the previous ADXL372 I2C write function
 *
 ******************************************************************************/
////////////////////////////////////////////////////////////////////////////////
void ADXL372Reset(void){
    int flag;
//--------- One full writing cycle for ADXL372 for Z Enable ------------------//
/*    '0' - NAK was received
 *    '1' - ACK was received, <---- This good
 *    '2' - timeout
 */
    ADXL372.start();
    flag = ADXL372.write(ADXL372_Address_8bit | 0);
    if(flag == 1)
    {
        //pc.printf("Write to I2C address success\n\r");
        
        flag = ADXL372.write(0x41);
        if(flag == 1)
        {
            //pc.printf("Write to 0x41 register address success\n\r");
            flag = ADXL372.write(0x52);                             // Set bit 0
            if(flag == 1)
            {
                pc.printf("Everything has been reset\n\r");
                ADXL372.stop();
            }
        }
    }
    else ADXL372.stop();
// ---------------- End of writing cycle --------------------------//
}
////////////////////////////////////////////////////////////////////////////////

/*
 *
 *  Self-test to see if the accelerometer is working as intended
 *  Wait 300 ms.
 *  Check bit 2 for a 1 for success. Bit 1 for completion of self-test.    
 *  Returns whole register
 */
////////////////////////////////////////////////////////////////////////////////
void I2CSelfTest(void){
    char *result;
    uint8_t check;
    accelerometerI2CWrite(0x3F, 0x03);
    accelerometerI2CWrite(0x40, 0x01);
    wait(0.3);
    result = accelerometerI2CRead(0x40);
    check = result[0];
    if(check & 0x04){
        pc.printf("Passed\n\r");
    }else {pc.printf("FAILED\n\r");}
}
////////////////////////////////////////////////////////////////////////////////

// Used to clear serial buffer in beginning
////////////////////////////////////////////////////////////////////////////////
void flushSerialBuffer(void) { char char1 = 0; while (pc.readable()) { char1 = pc.getc(); } return; }