Aidas Liaudanskas / Mbed 2 deprecated SmartGloves

CUED IIA Project

Dependencies:   BLE_API mbed nRF51822

Fork of BLE_GATT_Example by Bluetooth Low Energy

sensors.cpp@24:baa43caa2f3d, 2017-06-03 (annotated)

Committer:
AidasL
Date:
Sat Jun 03 21:14:42 2017 +0000
Revision:
24:baa43caa2f3d
Parent:
23:708cc5ef2604 
Saturday updates

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AidasL 23:708cc5ef2604 1 #include "sensors.h"
AidasL 23:708cc5ef2604 2 #include "mbed.h"
AidasL 23:708cc5ef2604 3 #include "wire.h"
AidasL 24:baa43caa2f3d 4
AidasL 23:708cc5ef2604 5 #define BLE_Nano
AidasL 24:baa43caa2f3d 6 //
AidasL 24:baa43caa2f3d 7 // #ifdef BLE_Nano
AidasL 24:baa43caa2f3d 8 // #define SCL 7
AidasL 24:baa43caa2f3d 9 // #define SDA 6
AidasL 24:baa43caa2f3d 10 // #endif
AidasL 24:baa43caa2f3d 11
AidasL 24:baa43caa2f3d 12 // #ifdef BLE_Nano
AidasL 24:baa43caa2f3d 13 // #define SCL 8 // D2
AidasL 24:baa43caa2f3d 14 // #define SDA 10 // D3
AidasL 24:baa43caa2f3d 15 // #endif
AidasL 24:baa43caa2f3d 16
AidasL 24:baa43caa2f3d 17
AidasL 24:baa43caa2f3d 18 // #define ACC2 0x69 // 0b01101001
AidasL 24:baa43caa2f3d 19
AidasL 24:baa43caa2f3d 20
AidasL 24:baa43caa2f3d 21
AidasL 24:baa43caa2f3d 22
AidasL 24:baa43caa2f3d 23 AnalogIn analogL(P0_4); // Analog senson input polling A3; Left In
AidasL 24:baa43caa2f3d 24 AnalogIn analogR(P0_5); // Analog senson input polling A4; Right In
AidasL 24:baa43caa2f3d 25
AidasL 24:baa43caa2f3d 26 DigitalOut flexA(P0_7); // Left Hand Finger Selectors: A
AidasL 24:baa43caa2f3d 27 DigitalOut flexB(P0_6); // B
AidasL 24:baa43caa2f3d 28 DigitalOut flexC(P0_15); // C
AidasL 24:baa43caa2f3d 29
AidasL 24:baa43caa2f3d 30 // DigitalOut flexRA(P0_28); // Right hand finder selectors : A
AidasL 24:baa43caa2f3d 31 // DigitalOut flexRB(P0_29); // B
AidasL 24:baa43caa2f3d 32 // DigitalOut flexRC(P0_11); // C
AidasL 24:baa43caa2f3d 33
AidasL 24:baa43caa2f3d 34 // TwoWire Wire = TwoWire(NRF_TWI0);
AidasL 24:baa43caa2f3d 35 I2C i2c(p8, p10);
AidasL 24:baa43caa2f3d 36
AidasL 24:baa43caa2f3d 37
AidasL 24:baa43caa2f3d 38
AidasL 24:baa43caa2f3d 39 void WriteBytes(uint8_t addr, uint8_t *pbuf, uint16_t length, uint8_t DEV_ADDR)
AidasL 24:baa43caa2f3d 40 {
AidasL 24:baa43caa2f3d 41 // Wire.beginTransmission(DEV_ADDR<<1);
AidasL 24:baa43caa2f3d 42 // Wire.write( (uint8_t)addr);
AidasL 24:baa43caa2f3d 43 // Wire.write(pbuf, length);
AidasL 24:baa43caa2f3d 44 // Wire.endTransmission();
AidasL 24:baa43caa2f3d 45 i2c.start();
AidasL 24:baa43caa2f3d 46 i2c.write( DEV_ADDR <<1 );
AidasL 24:baa43caa2f3d 47 i2c.write( addr );
AidasL 24:baa43caa2f3d 48 for (int i =0; i<length; i++) {
AidasL 24:baa43caa2f3d 49 i2c.write( *pbuf );
AidasL 24:baa43caa2f3d 50 pbuf++;
AidasL 24:baa43caa2f3d 51 }
AidasL 24:baa43caa2f3d 52 i2c.stop();
AidasL 24:baa43caa2f3d 53 }
AidasL 24:baa43caa2f3d 54
AidasL 24:baa43caa2f3d 55 void ReadBytes(uint8_t addr, uint8_t *pbuf, uint16_t length, uint8_t DEV_ADDR)
AidasL 24:baa43caa2f3d 56 {
AidasL 24:baa43caa2f3d 57 // Wire.beginTransmission((DEV_ADDR<<1)+1);
AidasL 24:baa43caa2f3d 58 // Wire.write( (uint8_t)addr);
AidasL 24:baa43caa2f3d 59 // Wire.endTransmission();
AidasL 24:baa43caa2f3d 60 // Wire.requestFrom(DEV_ADDR, length);
AidasL 24:baa43caa2f3d 61 // while( Wire.available() > 0 )
AidasL 24:baa43caa2f3d 62 i2c.start();
AidasL 24:baa43caa2f3d 63 i2c.write( (DEV_ADDR <<1) +1);
AidasL 24:baa43caa2f3d 64 i2c.write( addr );
AidasL 24:baa43caa2f3d 65 i2c.read( addr, (char*) pbuf, length);
AidasL 24:baa43caa2f3d 66 // {
AidasL 24:baa43caa2f3d 67 // *pbuf = Wire.read();
AidasL 24:baa43caa2f3d 68 // pbuf++;
AidasL 24:baa43caa2f3d 69 // }
AidasL 24:baa43caa2f3d 70 }
AidasL 24:baa43caa2f3d 71
AidasL 24:baa43caa2f3d 72 void WriteByte(uint8_t addr, uint8_t buf, uint8_t DEV_ADDR)
AidasL 24:baa43caa2f3d 73 {
AidasL 24:baa43caa2f3d 74 // Wire.beginTransmission(DEV_ADDR<<1);
AidasL 24:baa43caa2f3d 75 // Wire.write( (uint8_t)addr );
AidasL 24:baa43caa2f3d 76 // Wire.write((uint8_t)buf);
AidasL 24:baa43caa2f3d 77 // Wire.endTransmission();
AidasL 24:baa43caa2f3d 78 i2c.start();
AidasL 24:baa43caa2f3d 79 i2c.write( DEV_ADDR <<1 );
AidasL 24:baa43caa2f3d 80 i2c.write( addr );
AidasL 24:baa43caa2f3d 81 i2c.write( buf );
AidasL 24:baa43caa2f3d 82 i2c.stop();
AidasL 24:baa43caa2f3d 83 }
AidasL 24:baa43caa2f3d 84
AidasL 24:baa43caa2f3d 85 //
AidasL 24:baa43caa2f3d 86 //void Clear(){
AidasL 24:baa43caa2f3d 87 // Wire.twi_master_clear_bus()
AidasL 24:baa43caa2f3d 88 //}
AidasL 24:baa43caa2f3d 89
AidasL 24:baa43caa2f3d 90
AidasL 24:baa43caa2f3d 91 void setupI2C(void){
AidasL 24:baa43caa2f3d 92 // Wire.begin(SCL, SDA, TWI_FREQUENCY_100K);
AidasL 24:baa43caa2f3d 93 }
AidasL 24:baa43caa2f3d 94
AidasL 24:baa43caa2f3d 95 void setupacc(uint8_t ADDRESS){
AidasL 24:baa43caa2f3d 96 WriteByte(SMPRT_DIV, 0x13, ADDRESS); // Set rate divider to 19, so sample rate is 50hz
AidasL 24:baa43caa2f3d 97 WriteByte(CONFIG, 0x05, ADDRESS); // Set the digital low pass filter to 10Hz Cutoff for both Gyro and acc
AidasL 24:baa43caa2f3d 98 WriteByte(GYRO_CONFIG, 0x08, ADDRESS); // Set the gyro range to \pm 500 degrees/sec
AidasL 24:baa43caa2f3d 99 WriteByte(ACCEL_CONFIG, 0x00, ADDRESS); // Set the acc range to \pm 2g
AidasL 24:baa43caa2f3d 100 WriteByte(FIFO_EN, 0x00, ADDRESS); // Disable FIFO
AidasL 24:baa43caa2f3d 101 WriteByte(I2C_MST_CTRL, 0x00, ADDRESS); // Disabling external I2C
AidasL 24:baa43caa2f3d 102 WriteByte(INT_PIN_CFG, 0x30, ADDRESS); // Active high, push-pull, high until cleared, cleared on any read,
AidasL 24:baa43caa2f3d 103 WriteByte(INT_ENABLE, 0x00, ADDRESS); // DataRDY is the last bit if needed
AidasL 24:baa43caa2f3d 104 WriteByte(USER_CTRL, 0x00, ADDRESS); // No FIFO or I2C Master set
AidasL 24:baa43caa2f3d 105 WriteByte(PWR_MGMT_1, 0x00, ADDRESS); // TODO: Sleepmode is here; Disabled now; Cycle -> LP_WAKE_CTRL
AidasL 24:baa43caa2f3d 106 WriteByte(PWR_MGMT_2, 0x00, ADDRESS); // No lowpower mode, all sensors active
AidasL 24:baa43caa2f3d 107
AidasL 24:baa43caa2f3d 108 }
AidasL 24:baa43caa2f3d 109
AidasL 24:baa43caa2f3d 110 void readacc(Datapacket *data, uint8_t accadr){
AidasL 24:baa43caa2f3d 111 uint8_t rorl = 1; //
AidasL 24:baa43caa2f3d 112 if(accadr == ACC_RIGHT) {rorl=0; };
AidasL 24:baa43caa2f3d 113 uint8_t buffer[14]={0};
AidasL 24:baa43caa2f3d 114 uint16_t temp;
AidasL 24:baa43caa2f3d 115 ReadBytes(0x3B, buffer, 14, accadr);
AidasL 24:baa43caa2f3d 116 temp = ((buffer[0]<<8)+buffer[1]);
AidasL 24:baa43caa2f3d 117 for (int i=0; i<7; i++) {
AidasL 24:baa43caa2f3d 118
AidasL 24:baa43caa2f3d 119 temp=0;
AidasL 24:baa43caa2f3d 120 temp = ((buffer[2*i]<<8)+buffer[2*i + 1]);
AidasL 24:baa43caa2f3d 121 data->acc[rorl][i]= (uint16_t) temp;
AidasL 24:baa43caa2f3d 122 }
AidasL 24:baa43caa2f3d 123 // Acc data structure: ACC X, Y, Z; Temperature measurement; GYRO X, Y, Z;
AidasL 24:baa43caa2f3d 124
AidasL 24:baa43caa2f3d 125 }
AidasL 24:baa43caa2f3d 126
AidasL 24:baa43caa2f3d 127
AidasL 24:baa43caa2f3d 128 void readflexs(Datapacket *data){
AidasL 24:baa43caa2f3d 129
AidasL 24:baa43caa2f3d 130 // Order of the flex sensors:
AidasL 24:baa43caa2f3d 131 // 0 - right thumb, 4 - right pinky, 5 - R elbow;
AidasL 24:baa43caa2f3d 132 // 6 - left thumb, 10 left pinky, 11 - L elbow;
AidasL 24:baa43caa2f3d 133 // Read right hand in:
AidasL 24:baa43caa2f3d 134
AidasL 24:baa43caa2f3d 135 // AnalogIn analogL(P0_4); // Analog senson input polling A3; Left In
AidasL 24:baa43caa2f3d 136 // AnalogIn analogR(P0_5); // Analog senson input polling A4; Right In
AidasL 24:baa43caa2f3d 137 //
AidasL 24:baa43caa2f3d 138 // DigitalOut flexLA(P0_7); // Left Hand Finger Selectors: A
AidasL 24:baa43caa2f3d 139 // DigitalOut flexLB(P0_6); // B
AidasL 24:baa43caa2f3d 140 // DigitalOut flexLC(P0_15); // C
AidasL 24:baa43caa2f3d 141 //
AidasL 24:baa43caa2f3d 142 // DigitalOut flexRA(P0_28); // Right hand finder selectors : A
AidasL 24:baa43caa2f3d 143 // DigitalOut flexRB(P0_29); // B
AidasL 24:baa43caa2f3d 144 // DigitalOut flexRC(P0_11); // C
AidasL 24:baa43caa2f3d 145 // Address is CBA
AidasL 24:baa43caa2f3d 146 // Read thumbs:
AidasL 24:baa43caa2f3d 147 flexC = 0;
AidasL 24:baa43caa2f3d 148 flexB = 0;
AidasL 24:baa43caa2f3d 149 flexA = 0;
AidasL 24:baa43caa2f3d 150 data->flex[6] = (uint16_t) (analogL.read()*65535);
AidasL 24:baa43caa2f3d 151 // flexRC = 0;
AidasL 24:baa43caa2f3d 152 // flexRB = 0;
AidasL 24:baa43caa2f3d 153 // flexRA = 0;
AidasL 24:baa43caa2f3d 154 data->flex[0] = (uint16_t) (analogR.read()*65535);
AidasL 24:baa43caa2f3d 155
AidasL 24:baa43caa2f3d 156 flexC = 0;
AidasL 24:baa43caa2f3d 157 flexB = 0;
AidasL 24:baa43caa2f3d 158 flexA = 1;
AidasL 24:baa43caa2f3d 159 data->flex[7] = (uint16_t) (analogL.read()*65535);
AidasL 24:baa43caa2f3d 160 // flexRC = 0;
AidasL 24:baa43caa2f3d 161 // flexRB = 0;
AidasL 24:baa43caa2f3d 162 // flexRA = 1;
AidasL 24:baa43caa2f3d 163 data->flex[1] = (uint16_t) (analogR.read()*65535);
AidasL 24:baa43caa2f3d 164
AidasL 24:baa43caa2f3d 165 flexC = 0;
AidasL 24:baa43caa2f3d 166 flexB = 1;
AidasL 24:baa43caa2f3d 167 flexA = 0;
AidasL 24:baa43caa2f3d 168 data->flex[8] = (uint16_t) (analogL.read()*65535);
AidasL 24:baa43caa2f3d 169 // flexRC = 0;
AidasL 24:baa43caa2f3d 170 // flexRB = 1;
AidasL 24:baa43caa2f3d 171 // flexRA = 0;
AidasL 24:baa43caa2f3d 172 data->flex[2] = (uint16_t) (analogR.read()*65535);
AidasL 24:baa43caa2f3d 173
AidasL 24:baa43caa2f3d 174 flexC = 0;
AidasL 24:baa43caa2f3d 175 flexB = 1;
AidasL 24:baa43caa2f3d 176 flexA = 1;
AidasL 24:baa43caa2f3d 177 data->flex[9] = (uint16_t) (analogL.read()*65535);
AidasL 24:baa43caa2f3d 178 // flexRC = 0;
AidasL 24:baa43caa2f3d 179 // flexRB = 1;
AidasL 24:baa43caa2f3d 180 // flexRA = 1;
AidasL 24:baa43caa2f3d 181 data->flex[3] = (uint16_t) (analogR.read()*65535);
AidasL 24:baa43caa2f3d 182
AidasL 24:baa43caa2f3d 183 flexC = 1;
AidasL 24:baa43caa2f3d 184 flexB = 0;
AidasL 24:baa43caa2f3d 185 flexA = 0;
AidasL 24:baa43caa2f3d 186 data->flex[10] = (uint16_t) (analogL.read()*65535);
AidasL 24:baa43caa2f3d 187 // flexRC = 1;
AidasL 24:baa43caa2f3d 188 // flexRB = 0;
AidasL 24:baa43caa2f3d 189 // flexRA = 0;
AidasL 24:baa43caa2f3d 190 data->flex[4] = (uint16_t) (analogR.read()*65535);
AidasL 24:baa43caa2f3d 191
AidasL 24:baa43caa2f3d 192
AidasL 24:baa43caa2f3d 193 flexC = 1;
AidasL 24:baa43caa2f3d 194 flexB = 0;
AidasL 24:baa43caa2f3d 195 flexA = 1;
AidasL 24:baa43caa2f3d 196 data->flex[11] = (uint16_t) (analogL.read()*65535);
AidasL 24:baa43caa2f3d 197 // flexRC = 1;
AidasL 24:baa43caa2f3d 198 // flexRB = 0;
AidasL 24:baa43caa2f3d 199 // flexRA = 1;
AidasL 24:baa43caa2f3d 200 data->flex[5] = (uint16_t) (analogR.read()*65535);
AidasL 24:baa43caa2f3d 201
AidasL 24:baa43caa2f3d 202
AidasL 24:baa43caa2f3d 203 }