sensors.cpp

00001 #include "sensors.h"
00002 #include "mbed.h"
00003 #include "wire.h"
00004 
00005 #define BLE_Nano
00006 //
00007 // #ifdef BLE_Nano
00008 // #define SCL         7
00009 // #define SDA         6
00010 // #endif
00011 
00012 // #ifdef BLE_Nano
00013 // #define SCL         8 // D2
00014 // #define SDA         10 // D3
00015 // #endif
00016 
00017 
00018 // #define ACC2 0x69 //    0b01101001
00019 
00020 
00021 
00022 
00023 AnalogIn analogL(P0_4); // Analog senson input polling A3; Left In
00024 AnalogIn analogR(P0_5); // Analog senson input polling A4; Right In
00025 
00026 DigitalOut flexA(P0_7); // Left Hand Finger Selectors:  A
00027 DigitalOut flexB(P0_6); // B
00028 DigitalOut flexC(P0_15); //  C
00029 
00030 // DigitalOut flexRA(P0_28); //  Right hand finder selectors : A
00031 // DigitalOut flexRB(P0_29); //  B
00032 // DigitalOut flexRC(P0_11); //  C
00033 
00034 // TwoWire Wire = TwoWire(NRF_TWI0);
00035 I2C i2c(p8, p10);
00036 
00037 
00038 
00039 void WriteBytes(uint8_t addr, uint8_t *pbuf, uint16_t length, uint8_t DEV_ADDR)
00040 {
00041         // Wire.beginTransmission(DEV_ADDR<<1);
00042         // Wire.write( (uint8_t)addr);
00043         // Wire.write(pbuf, length);
00044         // Wire.endTransmission();
00045         i2c.start();
00046         i2c.write( DEV_ADDR <<1 );
00047         i2c.write( addr );
00048         for (int i =0; i<length; i++) {
00049                 i2c.write( *pbuf );
00050                 pbuf++;
00051         }
00052         i2c.stop();
00053 }
00054 
00055 void ReadBytes(uint8_t addr, uint8_t *pbuf, uint16_t length, uint8_t DEV_ADDR)
00056 {
00057         // Wire.beginTransmission((DEV_ADDR<<1)+1);
00058         // Wire.write( (uint8_t)addr);
00059         // Wire.endTransmission();
00060         // Wire.requestFrom(DEV_ADDR, length);
00061         // while( Wire.available() > 0 )
00062         i2c.start();
00063         i2c.write( (DEV_ADDR <<1) +1);
00064         i2c.write( addr );
00065         i2c.read( addr, (char*) pbuf, length);
00066         // {
00067         //         *pbuf = Wire.read();
00068         //         pbuf++;
00069         // }
00070 }
00071 
00072 void WriteByte(uint8_t addr, uint8_t buf, uint8_t DEV_ADDR)
00073 {
00074         // Wire.beginTransmission(DEV_ADDR<<1);
00075         // Wire.write( (uint8_t)addr );
00076         // Wire.write((uint8_t)buf);
00077         // Wire.endTransmission();
00078         i2c.start();
00079         i2c.write( DEV_ADDR <<1 );
00080         i2c.write( addr );
00081         i2c.write( buf );
00082         i2c.stop();
00083 }
00084 
00085 //
00086 //void Clear(){
00087 //  Wire.twi_master_clear_bus()
00088 //}
00089 
00090 
00091 void setupI2C(void){
00092         // Wire.begin(SCL, SDA, TWI_FREQUENCY_100K);
00093 }
00094 
00095 void setupacc(uint8_t ADDRESS){
00096         WriteByte(SMPRT_DIV, 0x13, ADDRESS); // Set rate divider to 19, so sample rate is 50hz
00097         WriteByte(CONFIG, 0x05, ADDRESS); // Set the digital low pass filter to 10Hz Cutoff for both Gyro and acc
00098         WriteByte(GYRO_CONFIG, 0x08, ADDRESS); // Set the gyro range to \pm 500 degrees/sec
00099         WriteByte(ACCEL_CONFIG, 0x00, ADDRESS); // Set the acc range to \pm 2g
00100         WriteByte(FIFO_EN, 0x00, ADDRESS); // Disable FIFO
00101         WriteByte(I2C_MST_CTRL, 0x00, ADDRESS); // Disabling external I2C
00102         WriteByte(INT_PIN_CFG, 0x30, ADDRESS); // Active high, push-pull, high until cleared, cleared on any read,
00103         WriteByte(INT_ENABLE, 0x00, ADDRESS); // DataRDY is the last bit if needed
00104         WriteByte(USER_CTRL, 0x00, ADDRESS); // No FIFO or I2C Master set
00105         WriteByte(PWR_MGMT_1, 0x00, ADDRESS); // TODO: Sleepmode is here; Disabled now; Cycle -> LP_WAKE_CTRL
00106         WriteByte(PWR_MGMT_2, 0x00, ADDRESS); // No lowpower mode, all sensors active
00107 
00108 }
00109 
00110 void readacc(Datapacket *data, uint8_t accadr){
00111         uint8_t rorl = 1; //
00112         if(accadr == ACC_RIGHT) {rorl=0; };
00113         uint8_t buffer[14]={0};
00114         uint16_t temp;
00115         ReadBytes(0x3B, buffer, 14, accadr);
00116         temp = ((buffer[0]<<8)+buffer[1]);
00117         for (int i=0; i<7; i++) {
00118 
00119                 temp=0;
00120                 temp = ((buffer[2*i]<<8)+buffer[2*i + 1]);
00121                 data->acc[rorl][i]= (uint16_t) temp;
00122         }
00123         // Acc data structure: ACC X, Y, Z; Temperature measurement; GYRO X, Y, Z;
00124 
00125 }
00126 
00127 
00128 void readflexs(Datapacket *data){
00129 
00130         // Order of the flex sensors:
00131         // 0 - right thumb, 4 - right pinky, 5 - R elbow;
00132         // 6 - left thumb, 10 left pinky, 11 - L elbow;
00133         // Read right hand in:
00134 
00135         // AnalogIn analogL(P0_4); // Analog senson input polling A3; Left In
00136         // AnalogIn analogR(P0_5); // Analog senson input polling A4; Right In
00137         //
00138         // DigitalOut flexLA(P0_7); // Left Hand Finger Selectors:  A
00139         // DigitalOut flexLB(P0_6); // B
00140         // DigitalOut flexLC(P0_15); //  C
00141         //
00142         // DigitalOut flexRA(P0_28); //  Right hand finder selectors : A
00143         // DigitalOut flexRB(P0_29); //  B
00144         // DigitalOut flexRC(P0_11); //  C
00145         // Address is CBA
00146         // Read thumbs:
00147         flexC = 0;
00148         flexB = 0;
00149         flexA = 0;
00150         data->flex[6] = (uint16_t) (analogL.read()*65535);
00151         // flexRC = 0;
00152         // flexRB = 0;
00153         // flexRA = 0;
00154         data->flex[0] = (uint16_t) (analogR.read()*65535);
00155 
00156         flexC = 0;
00157         flexB = 0;
00158         flexA = 1;
00159         data->flex[7] = (uint16_t) (analogL.read()*65535);
00160         // flexRC = 0;
00161         // flexRB = 0;
00162         // flexRA = 1;
00163         data->flex[1] = (uint16_t) (analogR.read()*65535);
00164 
00165         flexC = 0;
00166         flexB = 1;
00167         flexA = 0;
00168         data->flex[8] = (uint16_t) (analogL.read()*65535);
00169         // flexRC = 0;
00170         // flexRB = 1;
00171         // flexRA = 0;
00172         data->flex[2] = (uint16_t) (analogR.read()*65535);
00173 
00174         flexC = 0;
00175         flexB = 1;
00176         flexA = 1;
00177         data->flex[9] = (uint16_t) (analogL.read()*65535);
00178         // flexRC = 0;
00179         // flexRB = 1;
00180         // flexRA = 1;
00181         data->flex[3] = (uint16_t) (analogR.read()*65535);
00182 
00183         flexC = 1;
00184         flexB = 0;
00185         flexA = 0;
00186         data->flex[10] = (uint16_t) (analogL.read()*65535);
00187         // flexRC = 1;
00188         // flexRB = 0;
00189         // flexRA = 0;
00190         data->flex[4] = (uint16_t) (analogR.read()*65535);
00191 
00192 
00193         flexC = 1;
00194         flexB = 0;
00195         flexA = 1;
00196         data->flex[11] = (uint16_t) (analogL.read()*65535);
00197         // flexRC = 1;
00198         // flexRB = 0;
00199         // flexRA = 1;
00200         data->flex[5] = (uint16_t) (analogR.read()*65535);
00201 
00202 
00203 }