File content as of revision 24:baa43caa2f3d:

#include "sensors.h"
#include "mbed.h"
#include "wire.h"

#define BLE_Nano
//
// #ifdef BLE_Nano
// #define SCL         7
// #define SDA         6
// #endif

// #ifdef BLE_Nano
// #define SCL         8 // D2
// #define SDA         10 // D3
// #endif


// #define ACC2 0x69 //    0b01101001




AnalogIn analogL(P0_4); // Analog senson input polling A3; Left In
AnalogIn analogR(P0_5); // Analog senson input polling A4; Right In

DigitalOut flexA(P0_7); // Left Hand Finger Selectors:  A
DigitalOut flexB(P0_6); // B
DigitalOut flexC(P0_15); //  C

// DigitalOut flexRA(P0_28); //  Right hand finder selectors : A
// DigitalOut flexRB(P0_29); //  B
// DigitalOut flexRC(P0_11); //  C

// TwoWire Wire = TwoWire(NRF_TWI0);
I2C i2c(p8, p10);



void WriteBytes(uint8_t addr, uint8_t *pbuf, uint16_t length, uint8_t DEV_ADDR)
{
        // Wire.beginTransmission(DEV_ADDR<<1);
        // Wire.write( (uint8_t)addr);
        // Wire.write(pbuf, length);
        // Wire.endTransmission();
        i2c.start();
        i2c.write( DEV_ADDR <<1 );
        i2c.write( addr );
        for (int i =0; i<length; i++) {
                i2c.write( *pbuf );
                pbuf++;
        }
        i2c.stop();
}

void ReadBytes(uint8_t addr, uint8_t *pbuf, uint16_t length, uint8_t DEV_ADDR)
{
        // Wire.beginTransmission((DEV_ADDR<<1)+1);
        // Wire.write( (uint8_t)addr);
        // Wire.endTransmission();
        // Wire.requestFrom(DEV_ADDR, length);
        // while( Wire.available() > 0 )
        i2c.start();
        i2c.write( (DEV_ADDR <<1) +1);
        i2c.write( addr );
        i2c.read( addr, (char*) pbuf, length);
        // {
        //         *pbuf = Wire.read();
        //         pbuf++;
        // }
}

void WriteByte(uint8_t addr, uint8_t buf, uint8_t DEV_ADDR)
{
        // Wire.beginTransmission(DEV_ADDR<<1);
        // Wire.write( (uint8_t)addr );
        // Wire.write((uint8_t)buf);
        // Wire.endTransmission();
        i2c.start();
        i2c.write( DEV_ADDR <<1 );
        i2c.write( addr );
        i2c.write( buf );
        i2c.stop();
}

//
//void Clear(){
//  Wire.twi_master_clear_bus()
//}


void setupI2C(void){
        // Wire.begin(SCL, SDA, TWI_FREQUENCY_100K);
}

void setupacc(uint8_t ADDRESS){
        WriteByte(SMPRT_DIV, 0x13, ADDRESS); // Set rate divider to 19, so sample rate is 50hz
        WriteByte(CONFIG, 0x05, ADDRESS); // Set the digital low pass filter to 10Hz Cutoff for both Gyro and acc
        WriteByte(GYRO_CONFIG, 0x08, ADDRESS); // Set the gyro range to \pm 500 degrees/sec
        WriteByte(ACCEL_CONFIG, 0x00, ADDRESS); // Set the acc range to \pm 2g
        WriteByte(FIFO_EN, 0x00, ADDRESS); // Disable FIFO
        WriteByte(I2C_MST_CTRL, 0x00, ADDRESS); // Disabling external I2C
        WriteByte(INT_PIN_CFG, 0x30, ADDRESS); // Active high, push-pull, high until cleared, cleared on any read,
        WriteByte(INT_ENABLE, 0x00, ADDRESS); // DataRDY is the last bit if needed
        WriteByte(USER_CTRL, 0x00, ADDRESS); // No FIFO or I2C Master set
        WriteByte(PWR_MGMT_1, 0x00, ADDRESS); // TODO: Sleepmode is here; Disabled now; Cycle -> LP_WAKE_CTRL
        WriteByte(PWR_MGMT_2, 0x00, ADDRESS); // No lowpower mode, all sensors active

}

void readacc(Datapacket *data, uint8_t accadr){
        uint8_t rorl = 1; //
        if(accadr == ACC_RIGHT) {rorl=0; };
        uint8_t buffer[14]={0};
        uint16_t temp;
        ReadBytes(0x3B, buffer, 14, accadr);
        temp = ((buffer[0]<<8)+buffer[1]);
        for (int i=0; i<7; i++) {

                temp=0;
                temp = ((buffer[2*i]<<8)+buffer[2*i + 1]);
                data->acc[rorl][i]= (uint16_t) temp;
        }
        // Acc data structure: ACC X, Y, Z; Temperature measurement; GYRO X, Y, Z;

}


void readflexs(Datapacket *data){

        // Order of the flex sensors:
        // 0 - right thumb, 4 - right pinky, 5 - R elbow;
        // 6 - left thumb, 10 left pinky, 11 - L elbow;
        // Read right hand in:

        // AnalogIn analogL(P0_4); // Analog senson input polling A3; Left In
        // AnalogIn analogR(P0_5); // Analog senson input polling A4; Right In
        //
        // DigitalOut flexLA(P0_7); // Left Hand Finger Selectors:  A
        // DigitalOut flexLB(P0_6); // B
        // DigitalOut flexLC(P0_15); //  C
        //
        // DigitalOut flexRA(P0_28); //  Right hand finder selectors : A
        // DigitalOut flexRB(P0_29); //  B
        // DigitalOut flexRC(P0_11); //  C
        // Address is CBA
        // Read thumbs:
        flexC = 0;
        flexB = 0;
        flexA = 0;
        data->flex[6] = (uint16_t) (analogL.read()*65535);
        // flexRC = 0;
        // flexRB = 0;
        // flexRA = 0;
        data->flex[0] = (uint16_t) (analogR.read()*65535);

        flexC = 0;
        flexB = 0;
        flexA = 1;
        data->flex[7] = (uint16_t) (analogL.read()*65535);
        // flexRC = 0;
        // flexRB = 0;
        // flexRA = 1;
        data->flex[1] = (uint16_t) (analogR.read()*65535);

        flexC = 0;
        flexB = 1;
        flexA = 0;
        data->flex[8] = (uint16_t) (analogL.read()*65535);
        // flexRC = 0;
        // flexRB = 1;
        // flexRA = 0;
        data->flex[2] = (uint16_t) (analogR.read()*65535);

        flexC = 0;
        flexB = 1;
        flexA = 1;
        data->flex[9] = (uint16_t) (analogL.read()*65535);
        // flexRC = 0;
        // flexRB = 1;
        // flexRA = 1;
        data->flex[3] = (uint16_t) (analogR.read()*65535);

        flexC = 1;
        flexB = 0;
        flexA = 0;
        data->flex[10] = (uint16_t) (analogL.read()*65535);
        // flexRC = 1;
        // flexRB = 0;
        // flexRA = 0;
        data->flex[4] = (uint16_t) (analogR.read()*65535);


        flexC = 1;
        flexB = 0;
        flexA = 1;
        data->flex[11] = (uint16_t) (analogL.read()*65535);
        // flexRC = 1;
        // flexRB = 0;
        // flexRA = 1;
        data->flex[5] = (uint16_t) (analogR.read()*65535);


}