Diff: main.cpp

Revision:

7:b33be863fbb5

Parent:

5:f6727800e43f

--- a/main.cpp	Thu Dec 13 11:24:42 2018 +0000
+++ b/main.cpp	Mon Jan 28 09:00:47 2019 +0000
@@ -29,15 +29,14 @@
 * property whatsoever. Maxim Integrated Products, Inc. retains all
 * ownership rights.
 **********************************************************************/
-
-
 #include "mbed.h"
 #include "bmi160.h"
-//#include "max32630hsp.h"
+
 #include "max32630fthr.h"
 #include "USBSerial.h"
 #include "stdlib.h"
 #include "math.h"
+#include "Orientation.h"
 
 //MAX32630HSP icarus(MAX32630HSP::VIO_3V3);
 MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3);
@@ -51,291 +50,29 @@
 DigitalOut M_1(P3_5);
 DigitalOut M_2(P3_4);
 
-
-USBSerial pc(USBTX,USBRX);
-
-I2C i2cBus(I2C2_SDA, I2C2_SCL);
-
-
-
-BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO);
-
-
-void dumpImuRegisters(BMI160 &imu);
-void printRegister(BMI160 &imu, BMI160::Registers reg);
-void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg);
-void writeReg(BMI160 &imu, BMI160::Registers reg, uint8_t data);
-//float compFilter(float K, float pitch, float gyroX, float accY, float accZ,float DT);
-
-
-float compFilter(float K, float pitch, float gyroX, float accY, float accZ,float DT)
-{
-    return ((K * (pitch + (gyroX * DT))) + ((1.0F - K) * ((180.0F / 3.1459F) * atan(accY/accZ))));
-}
-
+Orientation orient1;
 
 int main()
 {
-    i2cBus.frequency(400000);
-    writeReg(imu, BMI160::GYR_RANGE, BMI160::DPS_500);
-    writeReg(imu, BMI160::GYR_CONF, BMI160::GYRO_ODR_13);
-    writeReg(imu, BMI160::FOC_CONF, BMI160::FOC_VALUE );
-    wait(0.5);
-    writeReg(imu, BMI160::OFFSET_6, BMI160::FOC_ENABLE_GYR_ACC);
     gLED = 1;
     bLED = 0;
     wait(1);
     
-    writeReg(imu, BMI160::CMD, BMI160::FOC_START);
+    
+    daplink.printf("Start...");
+    orient1.init();
     
     wait(1);
     gLED = 0;
     bLED = 0;
 
-    pc.printf("\033[H");  //home
-    pc.printf("\033[0J");  //erase from cursor to end of screen
-
     uint32_t failures = 0;
 
-    if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
-    {
-        pc.printf("Failed to set gyroscope power mode\r\n");
-        failures++;
-    }
-    wait_ms(100);
-
-    if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
-    {
-        pc.printf("Failed to set accelerometer power mode\r\n");
-        failures++;
-    }
-    wait_ms(100);
-
-
-    BMI160::AccConfig accConfig;
-    //example of using getSensorConfig
-    if(imu.getSensorConfig(accConfig) == BMI160::RTN_NO_ERROR)
-    {
-        pc.printf("ACC Range = %d\r\n", accConfig.range);
-        pc.printf("ACC UnderSampling = %d\r\n", accConfig.us);
-        pc.printf("ACC BandWidthParam = %d\r\n", accConfig.bwp);
-        pc.printf("ACC OutputDataRate = %d\r\n\r\n", accConfig.odr);
-    }
-    else
-    {
-        pc.printf("Failed to get accelerometer configuration\r\n");
-        failures++;
-    }
-
-    //example of setting user defined configuration
-    accConfig.range = BMI160::SENS_2G;  //rage is 2g
-    accConfig.us = BMI160::ACC_US_OFF;  //undersampling is off 
-    accConfig.bwp = BMI160::ACC_BWP_0;  //average 4 cycles
-    accConfig.odr = BMI160::ACC_ODR_9;  //output data rate
-    if(imu.setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR)
-    {
-        pc.printf("ACC Range = %d\r\n", accConfig.range);
-        pc.printf("ACC UnderSampling = %d\r\n", accConfig.us);
-        pc.printf("ACC BandWidthParam = %d\r\n", accConfig.bwp);
-        pc.printf("ACC OutputDataRate = %d\r\n\r\n", accConfig.odr);
-    }
-    else
-    {
-        pc.printf("Failed to set accelerometer configuration\r\n");
-        failures++;
-    }
-
-    BMI160::GyroConfig gyroConfig;
-    if(imu.getSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR)
+     
+    while(1)
     {
-        pc.printf("GYRO Range = %d\r\n", gyroConfig.range);
-        pc.printf("GYRO BandWidthParam = %d\r\n", gyroConfig.bwp);
-        pc.printf("GYRO OutputDataRate = %d\r\n\r\n", gyroConfig.odr);
-        
-        
-    }
-    else
-    {
-        pc.printf("Failed to get gyroscope configuration\r\n");
-        failures++;
-    }
-
-    wait(1.0);
-    pc.printf("\033[H");  //home
-    pc.printf("\033[0J");  //erase from cursor to end of screen
-
-    if(failures == 0)
-    {
-        
-        
-        float imuTemperature;
-        
-        double xDeviation, yDeviation, zDeviation;
-        double prevGyroX, prevGyroY, prevGyroZ;
-        double currentGyroX, currentGyroY, currentGyroZ;
-        double diffGyroX, diffGyroY, diffGyroZ;
-        
-        double xDisplacement, yDisplacement, zDisplacement;
-        double currentAccX, currentAccY, currentAccZ;
-        double prevAccX, prevAccY, prevAccZ;
-        double diffAccX, diffAccY, diffAccZ;
-        
-        double xVelocity, yVelocity, zVelocity;
-        double timeDiff, time_1, time_2;
-        bool timeFlag = false;
-        BMI160::SensorData accData;
-        BMI160::SensorData gyroData;
-        BMI160::SensorTime sensorTime;
-        
-        //PwmPin = 1;
-        float apitch = 0;
-        float k = 0.6;
-        
-        time_1 = sensorTime.seconds;
-        while(1)
-        {
-            
-            imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
-            imu.getTemperature(&imuTemperature);
-            
-            time_2 = sensorTime.seconds;
-            apitch = compFilter(k, apitch, gyroData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled, time_2 - time_1);
-            daplink.printf("Forward: %s%4.3f\r\n", "\033[K", apitch);
-            time_1 = time_2;
-            
-             //printRegister(imu, BMI160::GYR_CONF);
-             
-       }
+        orient1.updatePitch();
+        daplink.printf("Forward: %s%4.3f\r\n", "\033[K", orient1.getPitch());
     }
 }     
              
-/*             
-            if(timeFlag == true){
-                
-                currentGyroX = gyroData.xAxis.scaled;
-                currentAccX = accData.xAxis.scaled;
-
-                
-                diffGyroX = abs(currentGyroX - prevGyroX);
-                diffAccX = abs(currentAccX - prevAccX);
-                
-                time_2 = sensorTime.seconds;
-                timeDiff = time_2 - time_1;
-    
-                if (diffGyroX > 2){
-                    xDeviation = xDeviation + (gyroData.xAxis.scaled * (timeDiff)); 
-                }  
-                if (diffAccX > 0.009){
-                    xDisplacement = (xVelocity * timeDiff) + (0.5 * accData.xAxis.scaled * timeDiff * timeDiff);
-                    xVelocity = xVelocity + (accData.xAxis.scaled * timeDiff);
-                }
-        
-               // pc.printf("%s%4.3f\r\n", "\033[K", xDeviation);
-                
-                //control motor for proportional linearity
-                
-                    if(xDeviation < 0.0)
-                {
-                    M_1 = 0;
-                    M_2 = 1;
-                    daplink.printf("Forward: %s%4.3f\r\n", "\033[K", xDeviation);
-                }
-                    else{
-                    M_1 = 1;
-                    M_2 = 0;
-                    daplink.printf("Backward: %s%4.3f\r\n", "\033[K", xDeviation);
-                }
-                
-                //------------------------
-                
-                
-                //pc.printf("Velocity: %s%4.3f\r\n", "\033[K", prevAccX);
-               // pc.printf("Interval: %s%4.3f\r\n", "\033[K", timeDiff);
-                //pc.printf("%s%4.3f\r\n\r\n", "\033[K", xDisplacement);
-
-                prevGyroX = currentGyroX;
-                prevAccX = currentAccX;
-                time_1 = time_2;
-            }
-            else{
-                time_1 = sensorTime.seconds;
-                timeFlag = true;
-            }
-        
-            
-            gLED = !gLED;
-            wait_ms(1);
-        }
-    }
-    else
-    {
-        while(1)
-        {
-            rLED = !rLED;
-            wait(0.6);
-        }
-    }
-*/
-
-
-
-
-//*****************************************************************************
-void dumpImuRegisters(BMI160 &imu)
-{
-    printRegister(imu, BMI160::CHIP_ID);
-    printBlock(imu, BMI160::ERR_REG,BMI160::FIFO_DATA);
-    printBlock(imu, BMI160::ACC_CONF, BMI160::FIFO_CONFIG_1);
-    printBlock(imu, BMI160::MAG_IF_0, BMI160::SELF_TEST);
-    printBlock(imu, BMI160::NV_CONF, BMI160::STEP_CONF_1);
-    printRegister(imu, BMI160::CMD);
-    pc.printf("\r\n");
-}
-
-
-//*****************************************************************************
-void printRegister(BMI160 &imu, BMI160::Registers reg)
-{
-    uint8_t data;
-    if(imu.readRegister(reg, &data) == BMI160::RTN_NO_ERROR)
-    {
-        pc.printf("IMU Register 0x%02x = 0x%02x\r\n", reg, data);
-         daplink.printf("IMU Register 0x%02x = 0x%02x\r\n", reg, data);
-    }
-    else
-    {
-        pc.printf("Failed to read register\r\n");
-    }
-}
-
-//*****************************************************************************
-void writeReg(BMI160 &imu, BMI160::Registers reg, uint8_t data)
-{
-    imu.writeRegister(reg, data);
-    
-    
-}
-
-
-//*****************************************************************************
-void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg)
-{
-    uint8_t numBytes = ((stopReg - startReg) + 1);
-    uint8_t buff[32];
-    uint8_t offset = static_cast<uint8_t>(startReg);
-
-    if(imu.readBlock(startReg, stopReg, buff) == BMI160::RTN_NO_ERROR)
-    {
-        for(uint8_t idx = offset; idx < (numBytes + offset); idx++)
-        {
-            pc.printf("IMU Register 0x%02x = 0x%02x\r\n", idx, buff[idx - offset]);
-        }
-    }
-    else
-    {
-        pc.printf("Failed to read block\r\n");
-    }
-}
-
-
-/* An example for configuring FOC for accel and gyro data */