CUED IIA Project
Dependencies: BLE_API mbed nRF51822
Fork of BLE_GATT_Example by
Diff: sensors.cpp
- Revision:
- 24:baa43caa2f3d
- Parent:
- 23:708cc5ef2604
--- a/sensors.cpp Wed May 31 22:13:36 2017 +0000 +++ b/sensors.cpp Sat Jun 03 21:14:42 2017 +0000 @@ -1,12 +1,203 @@ #include "sensors.h" #include "mbed.h" #include "wire.h" - + #define BLE_Nano - -#ifdef BLE_Nano -#define SCL 7 -#define SDA 6 -#endif - -#define DEV_ADDR 0xA0 \ No newline at end of file +// +// #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); + + +}