CUED IIA Project
Dependencies: BLE_API mbed nRF51822
Fork of BLE_GATT_Example by
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?
User | Revision | Line number | New 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 | } |