Diff: Sensors.cpp

Revision:

6:75263c93daf7

Parent:

5:46947b447701

Child:

8:c6345e8d607c

--- a/Sensors.cpp	Sun Jun 07 14:11:26 2015 +0000
+++ b/Sensors.cpp	Sun Jun 07 21:58:02 2015 +0000
@@ -2,42 +2,110 @@
 
 I2C i2c(p29, p28);
 
-Sensors* Sensors::instance = new Sensors();
-
 const int AS5600 = 0x36 << 1;
-const int MPU6500 = 0x68 << 1;
+const int MPU6050 = 0x68 << 1;
 
 Sensors::Sensors()
 {
-    i2c.frequency(250000);
-
-    this->angle_h = 0;
-    this->angle_l = 0;
+    i2c.frequency(100000);
     
-    this->accel_x_h = 0;
-    this->accel_y_h = 0;
-    this->accel_z_h = 0;
-    this->accel_x_l = 0;
-    this->accel_y_l = 0;
-    this->accel_z_l = 0;
+    for(int i = 0; i < 12; i++){
+        imuData[i] = 0;
+        }
+        
+    angle[0] = angle[1] = 0;
     
-    this->gyro_x_h = 0;
-    this->gyro_y_h = 0;
-    this->gyro_z_h = 0;
-    this->gyro_x_l = 0;
-    this->gyro_y_l = 0;
-    this->gyro_z_l = 0;
+    setupAngle();
+}
 
+void Sensors::setupAngle()
+{
+    cmd[0] = 0x0C;
+    i2c.write(AS5600,cmd,1);
+    i2c.read(AS5600,out,2);
+    
+    cmd[0]= 0x01;
+    cmd[1] = out[0];
+    cmd[2] = out[1];
+    i2c.write(AS5600,cmd,3);
+    
+}
 
-    //setupIMU();
-    setupAngle();
-    //disableIMU();
+void Sensors::wakeIMU(){
+    writeRegister(0x6B, (readRegister(0x6B) & 0xbf));
+}
+
+void Sensors::disableIMU(){
+    writeRegister(0x6B, (readRegister(0x6B) | (1 << 6)));
 }
 
 
-Sensors* Sensors::getInstance()
+void Sensors::setupIMU()
 {
-    return instance;
+    //ResetIMU
+    //writeRegister(0x6B, readRegister(0x6B) | (1<<7));
+    //wait(0.1);
+    
+    
+    //Set sample rate to 8000/1+79 = 100Hz
+    writeRegister(0x19,0x4f); //4f = 100 hz, 9f = 50 hz
+
+    //Disable Frame Synchronization (FSYNC)
+    writeRegister(0x1A,0x0);
+
+    //Gyroscope Configuration
+    //bit 4 and 3 is the full scale
+    //0 = 250, 1 = 500, 2 = 1000, 3 = 2000
+    writeRegister(0x1B,(0x0 << 3));
+
+    //Accelerometer Configuration
+    //bit 4 and 3 is the full scale
+    //0 = 2g, 1 = 4g, 2 = 8g, 3 = 16g
+    writeRegister(0x1C,(0x3 << 3));
+
+    //Set Motion Free Fall Threshold
+    writeRegister(0x1D,0x01);
+
+    //Set Motion Free Fall Dur
+    writeRegister(0x1E,0xff);
+
+    //Set Motion Detection Threshold
+    writeRegister(0x1F,0x5);
+
+    //Set Motion Detection Dur
+    writeRegister(0x20,0x01);
+
+    //Set Zero Motion Threshold
+    writeRegister(0x21,0xf1);
+
+    //Set Zero Motion Dur
+    writeRegister(0x22,0x01);
+
+    //Disable FIFO buffer
+    writeRegister(0x23,0x00);
+
+    //Set aux I2C, from 0x24 to 0x36 = 0x00
+
+    //Setup INT pin and AUX I2C pass through
+    writeRegister(0x37,0x02);
+
+    //Interrupt Enable
+    //[7] FF_EN [6] MOT_EN  [5] ZMOT_EN [4] FIFO_OFLOW_EN   [3] I2C_MST_INT_EN  [2] PLL_RDY_INT_EN  [1] DMP_INT_EN  [0] RAW_RDY_EN
+    writeRegister(0x38,0xe1); 
+
+    //Motion detection control
+    writeRegister(0x69,(1<<4));
+
+    //User control
+    //[7] DMP_EN    [6] FIFO_EN [5] I2C_MST_EN  [4] I2C_IF_DIS  [3] DMP_RESET   [2] FIFO_RESET  [1] I2C_MST_RESET   [0] SIG_COND_RESET
+    writeRegister(0x6A,0x00);
+
+    //Set power managenemt 1
+    //[7] DEVICE_RESET    [6] SLEEP   [5] CYCLE       [3] TEMP_DIS    [2:0] CLK_SEL
+    writeRegister(0x6B,0x02);
+
+    //Set power managenemt 2
+    writeRegister(0x6C,0x00);
 }
 
 void Sensors::updateAngle()
@@ -46,182 +114,51 @@
     cmd[0] = 0x0E;
     i2c.write(AS5600,cmd,1);
     i2c.read(AS5600,out,2);
-
-    this->angle_h = out[0];   //((out[0] << 8) + out[1]) * 0.087912087;
-    this->angle_l = out[1];
-}
-
-float Sensors::getAngle(){
-    return ((getAngleH() << 8) + getAngleL()) * 0.087912087;
-    }
-
-char Sensors::getAngleH(){
-    return this->angle_h;
-}
-char Sensors::getAngleL(){
-    return this->angle_l;
-}
-
-void Sensors::setupAngle()
-{
-    //TODO
-}
-
-void Sensors::wakeIMU(){
-    writeRegister(0x6B, (readRegister(0x6B) & 0xbf));
-    }
     
-void Sensors::disableIMU(){
-    writeRegister(0x6B, (readRegister(0x6B) | 1 << 6));
-    }
-    
-int8_t Sensors::getFs(){
-    return 8000 / (readRegister(0x19) + 1);
-    }
-
-void Sensors::setupIMU()
-{
-    //Sets sample rate to 8000/1+79 = 100Hz
-    writeRegister(0x19,0x4f); //4f = 100 hz, 9f = 50 hz
-    //Disable FSync, 256 DLPF
-    writeRegister(0x1A,0x00);
-    
-    // Disable gyroscope self tests, scale of 200 degrees/s
-    // Disable accelerometer self tests, scale of +-2g, no DHPF
-    cmd[0] = 0x1B;
-    cmd[1] = 0x00;  //bit4 and bit 3 to choose scale range
-    cmd[2] = 0x00;
-    i2c.write(MPU6500,cmd,3);
-    
-    //Freefall threshold of 0mg and duration limit of 0
-    cmd[0] = 0x1D;
-    i2c.write(MPU6500,cmd,3);
-    
-    //Motion threshold of 0mg and duration limit of 0
-    cmd[0] = 0x21;
-    i2c.write(MPU6500,cmd,3);
-    
-    //Disable sensor output FIFO buffer
-    writeRegister(0x23,0x00);
-    
-    //Set aux I2C, from 0x24 to 0x36 = 0x00 
-    
-    //Setup INT pin and AUX I2C pass through
-    cmd[0] = 0x37;
-    cmd[1] = 0x02;
-    cmd[2] = 0x01; 
-    i2c.write(MPU6500,cmd,3);
-    
-    //Reset sensor signal paths
-    writeRegister(0x68,0x00);
-    
-    //Motion detection control
-    writeRegister(0x69,0x00);
-    
-    //Disables FIFO, AUX I2C and I2C reset bits to 0
-    writeRegister(0x6A,0x00);
-    
-    //Sets clock source to gyro reference w/ PLL
-    /* SLEEP = 0 */
-    writeRegister(0x6B,0x02);
-    
-    //Controls frequency of wakeups in accel low power mode plus the sensor standby modes
-    writeRegister(0x6C,0x00);
-
-    //Data transfer to and from the FIFO buffer
-    writeRegister(0x74,0x00);
+    angle[0] = out[0];
+    angle[1] = out[1];
 }
 
 void Sensors::updateIMU()
 {
-    // Read measurements from MPU6500  
     cmd[0] = 0x3B;
-    i2c.write(MPU6500,cmd,1,true);
-    i2c.read(MPU6500,out,14);
+    i2c.write(MPU6050,cmd,1,true);
+    i2c.read(MPU6050,out,14);
     
-    this->accel_x_h = out[0];
-    this->accel_x_l = out[1];
-    this->accel_y_h = out[2];
-    this->accel_y_l = out[3];
-    this->accel_z_h = out[4];
-    this->accel_z_l = out[5];
-        
-    int16_t temp16 = ((out[6] << 8) | out[7]);
+    imuData[0] = out[0];
+    imuData[1] = out[1];
+    imuData[2] = out[2];
+    imuData[3] = out[3];
+    imuData[4] = out[4];
+    imuData[5] = out[5];
     
-    this->temp = temp16/340 + 36.53;
-
-    this->gyro_x_h = out[8];
-    this->gyro_x_l = out[9];
-    this->gyro_y_h = out[10];
-    this->gyro_y_l = out[11];
-    this->gyro_z_h = out[12];
-    this->gyro_z_l = out[13];
-    
+    imuData[6] = out[8];
+    imuData[7] = out[9];
+    imuData[8] = out[10];
+    imuData[9] = out[11];
+    imuData[10] = out[12];
+    imuData[11] = out[13];
 }
 
-int8_t Sensors::getAccelXH(){
-    return this->accel_x_h;
-}
-int8_t Sensors::getAccelXL(){
-    return this->accel_x_l;
-}
-int8_t Sensors::getAccelYH(){
-    return this->accel_y_h;
-}
-int8_t Sensors::getAccelYL(){
-    return this->accel_y_l;
-}
-int8_t Sensors::getAccelZH(){
-    return this->accel_z_h;
-}
-int8_t Sensors::getAccelZL(){
-    return this->accel_z_l;
+int8_t Sensors::getInterruptStatus(){
+    //[7] FF_INT  [6] MOT_INT [5] ZMOT_INT    [4] FIFO_OFLOW_INT  [3] I2C_MST_INT [2] PLL_RDY_INT [1] DMP_INT [0] RAW_RDY_IN
+    return readRegister(0x3A);
 }
 
+uint8_t Sensors::getAngle(int i){
+    return angle[i];
+    }
 
-int8_t Sensors::getGyroXH(){
-    return this->gyro_x_h;
-}
-int8_t Sensors::getGyroXL(){
-    return this->gyro_x_l;
-}
-int8_t Sensors::getGyroYH(){
-    return this->gyro_y_h;
-}
-int8_t Sensors::getGyroYL(){
-    return this->gyro_y_l;
-}
-int8_t Sensors::getGyroZH(){
-    return this->gyro_z_h;
-}
-int8_t Sensors::getGyroZL(){
-    return this->gyro_z_l;
-}
+int8_t Sensors::getIMU(int i){
+    return imuData[i];
+    }
 
 
-int16_t Sensors::getAccelX(){
-    return ((getAccelXH() << 8) + getAccelXL());
-    }
-int16_t Sensors::getAccelY(){
-    return ((getAccelYH() << 8) + getAccelYL());
-    }
-int16_t Sensors::getAccelZ(){
-    return ((getAccelZH() << 8) + getAccelZL());
-    }
-    
-int16_t Sensors::getGyroX(){
-    return ((getGyroXH() << 8) + getGyroXL());
-    }
-    
-float Sensors::getTemp(){
-    return this->temp;
-    }
-
 int8_t Sensors::readRegister(int8_t addr)
 {
     cmd[0] = addr;
-    i2c.write(MPU6500,cmd,1);
-    i2c.read(MPU6500,out,1);
+    i2c.write(MPU6050,cmd,1);
+    i2c.read(MPU6050,out,1);
     return out[0];
 }
 
@@ -229,13 +166,13 @@
 {
     cmd[0] = addr;
     cmd[1] = value;
-    i2c.write(MPU6500,cmd,2);
+    i2c.write(MPU6050,cmd,2);
 }
 
+//http://www.i2cdevlib.com/devices/mpu6050#registers
+
 
 
 
 
 
-
-