Diff: main.cpp

Revision:

0:52b06cb96c86

Child:

1:bae4f01ae25c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Apr 15 02:10:41 2018 +0000
@@ -0,0 +1,340 @@
+#include "mbed.h"
+#include "tca9548a.h"
+#include "VL53L0X.h"
+#include "LSM9DS1.h"
+#define PI 3.14159
+#define DECLINATION -4.94
+
+PwmOut PWM(p21);            //Right Side Motor
+PwmOut PWM1(p22);           //Left Side Motor
+Serial pc(USBTX, USBRX);
+LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);  //imu
+TCA9548A i2c_sw(P0_27, P0_28); //default address 0x70 applied
+VL53L0X ToF(P0_27, P0_28);     //tof
+
+float dutyLeft=0.130;
+float dutyRight=0.130;
+float zeroDegreeHeading;
+float currentDegreeHeading;
+float distanceAway;
+
+/*
+#include "LSM9DS1.h"
+ 
+DigitalOut myled(LED1);
+Serial pc(USBTX, USBRX);
+
+int main() {
+    LSM9DS1 imu(p9, p10, 0xD6, 0x3C);
+    imu.begin();
+    if (!imu.begin()) {
+        pc.printf("Failed to communicate with LSM9DS1.\n");
+    }
+    imu.calibrate();
+    while(1) {
+        imu.readTemp();
+        imu.readMag();
+        imu.readGyro();
+
+        pc.printf("gyro: %d %d %d\n\r", imu.gx, imu.gy, imu.gz);
+        pc.printf("accel: %d %d %d\n\r", imu.ax, imu.ay, imu.az);
+        pc.printf("mag: %d %d %d\n\n\r", imu.mx, imu.my, imu.mz);
+        wait(1);
+    }
+}
+*/
+
+/*
+#include "mbed.h"
+#include "tca9548a.h"
+#include "VL53L0X.h"
+
+TCA9548A i2c_sw(I2C_SDA, I2C_SCL); //default address 0x70 applied
+VL53L0X ToF(I2C_SDA, I2C_SCL);
+int main()
+{
+    // By default TCA9548A performs a power on reset and all downstream ports are deselected
+
+    i2c_sw.select(0);               //  select  the channel 0
+    ToF.init();
+    ToF.startContinuous();
+    i2c_sw.select(1);               //  select  the channel 0
+    ToF.init();
+    ToF.startContinuous();
+    //i2c_sw.select(1);               //  select  the channel 1
+    int counter=0;
+    while(1) {
+        printf("Tof 1:%d mm \t",ToF.readRangeContinuousMillimeters());
+        i2c_sw.select(1);
+        printf("Tof 2:%d mm \n",ToF.readRangeContinuousMillimeters());
+        i2c_sw.select(0);
+        wait( 0.1 );
+    }
+}
+*/
+float calculateHeading(float ax, float ay, float az, float mx, float my, float mz){
+    float roll = atan2(ay, az);
+    float pitch = atan2(-ax, sqrt(ay * ay + az * az));
+// touchy trig stuff to use arctan to get compass heading (scale is 0..360)
+    mx = -mx;
+    float heading;
+    if (my == 0.0)
+        heading = (mx < 0.0) ? 180.0 : 0.0;
+    else
+        heading = atan2(mx, my)*360.0/(2.0*PI);
+    //pc.printf("heading atan=%f \n\r",heading);
+    heading -= DECLINATION; //correct for geo location
+    if(heading>180.0) heading = heading - 360.0;
+    else if(heading<-180.0) heading = 360.0 + heading;
+    else if(heading<0.0) heading = 360.0  + heading;
+
+    pitch *= 180.0 / PI;
+    roll  *= 180.0 / PI;
+
+    return heading;
+    }
+
+
+void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
+{
+    float roll = atan2(ay, az);
+    float pitch = atan2(-ax, sqrt(ay * ay + az * az));
+// touchy trig stuff to use arctan to get compass heading (scale is 0..360)
+    mx = -mx;
+    float heading;
+    if (my == 0.0)
+        heading = (mx < 0.0) ? 180.0 : 0.0;
+    else
+        heading = atan2(mx, my)*360.0/(2.0*PI);
+    heading -= DECLINATION; //correct for geo location
+    if(heading>180.0) heading = heading - 360.0;
+    else if(heading<-180.0) heading = 360.0 + heading;
+    else if(heading<0.0) heading = 360.0  + heading;
+
+    pitch *= 180.0 / PI;
+    roll  *= 180.0 / PI;
+
+    pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch,roll);
+    pc.printf("Magnetic Heading: %f degress\n\r",heading);
+}
+
+void calibrateIMU()
+{
+    IMU.begin();
+    if (!IMU.begin()) {
+        pc.printf("Failed to communicate with LSM9DS1.\n");
+    }
+    IMU.calibrate(1);
+    IMU.calibrateMag(0);    
+    return;
+}
+
+void initToF ()
+{
+    // By default TCA9548A performs a power on reset and all downstream ports are deselected
+    
+    i2c_sw.select(0);  
+    pc.printf("before init1\n");             //  select  the channel 0
+    ToF.init();
+    pc.printf("inside init1\n");
+    ToF.startContinuous();
+    i2c_sw.select(1);               //  select  the channel 0
+    pc.printf("inside init2\n");
+    ToF.init();
+    pc.printf("inside init2\n");
+    ToF.startContinuous();
+    
+
+    //i2c_sw.select(1);               //  select  the channel 1
+}
+
+void readToF()
+{
+    printf("Tof 1:%d mm \t",ToF.readRangeContinuousMillimeters());
+    i2c_sw.select(1);
+    printf("Tof 2:%d mm \n",ToF.readRangeContinuousMillimeters());
+    i2c_sw.select(0);
+    wait( 0.1 );
+}
+
+void readIMU()
+{
+    while(1){
+    while(!IMU.tempAvailable());
+        IMU.readTemp();
+        while(!IMU.magAvailable(X_AXIS));
+        IMU.readMag();
+        while(!IMU.accelAvailable());
+        IMU.readAccel();
+        while(!IMU.gyroAvailable());
+        IMU.readGyro();
+        //pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0);
+        //pc.printf("        X axis    Y axis    Z axis\n\r");
+        //pc.printf("gyro:  %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
+        //pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
+        pc.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
+        printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
+                      IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
+        //myled = 1;
+        wait(0.25);
+        //myled = 0;
+        wait(0.25);
+        }
+}
+
+void turnRight()
+{
+    dutyLeft = 0.18;
+    dutyRight = 0.05;              //turn right
+    PWM.write(dutyRight);
+    PWM1.write(dutyLeft);
+    pc.printf("Duty Cycle Left: %f\t Duty Cycle Right: %f\r\n", dutyLeft, dutyRight);
+}
+
+void turnLeft()
+{
+    dutyRight = 0.18;            //increase right motor speed
+    dutyLeft = 0.05;              //Stop left motor
+    PWM.write(dutyRight);
+    PWM1.write(dutyLeft);
+    pc.printf("Duty Cycle Left: %f\t Duty Cycle Right: %f\r\n", dutyLeft, dutyRight);
+}
+
+void goForward()
+{
+    if(dutyRight < dutyLeft) {
+        dutyLeft = dutyRight;
+    }
+    if(dutyLeft < dutyRight) {
+        dutyRight = dutyLeft;
+    }
+    dutyRight = 0.18;            
+    dutyLeft = 0.18;
+    PWM.write(dutyRight);
+    PWM1.write(dutyLeft);
+    pc.printf("Duty Cycle Left: %f\t Duty Cycle Right: %f\r\n", dutyLeft, dutyRight);
+}
+
+void slowDown()
+{
+    if(dutyRight >=0.130) {
+        dutyRight -= 0.001;
+    }
+    if(dutyLeft >=0.130) {
+        dutyLeft -= 0.001;
+    }
+    PWM.write(dutyRight);
+    PWM1.write(dutyLeft);
+    pc.printf("Duty Cycle Left: %f\t Duty Cycle Right: %f\r\n", dutyLeft, dutyRight);
+}
+
+void stop()
+{
+    dutyLeft = dutyRight = 0.13;
+    PWM.write(dutyRight);
+    PWM1.write(dutyLeft);           //stop motor
+    pc.printf("Duty Cycle Left: %f\t Duty Cycle Right: %f\r\n", dutyLeft, dutyRight);
+}
+
+void steady()
+{
+    PWM.write(dutyRight);
+    PWM1.write(dutyLeft);
+}
+
+void startupESC()
+{
+    PWM.write(0.01);
+    PWM1.write(0.01);
+    wait(0.5);
+    PWM.write(1.0);
+    PWM1.write(1.0);
+    wait(8);
+    PWM.write(0.01);
+    PWM1.write(0.01);
+    wait(8);
+}
+
+void maintainHeading(float desiredHeading){
+    while(!IMU.tempAvailable());
+    IMU.readTemp();
+    while(!IMU.magAvailable(X_AXIS));
+    IMU.readMag();
+    while(!IMU.accelAvailable());
+    IMU.readAccel();
+    while(!IMU.gyroAvailable());
+    IMU.readGyro();
+    currentDegreeHeading = calculateHeading(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
+                                            IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
+    pc.printf("heading: %f", currentDegreeHeading);
+    currentDegreeHeading = fmod(((double)currentDegreeHeading-desiredHeading),360.0);
+    if(currentDegreeHeading<0.0){currentDegreeHeading=360.0-fabs(currentDegreeHeading);}
+    pc.printf(" heading: %f\n", currentDegreeHeading);
+    
+    if(currentDegreeHeading < 180.0 && currentDegreeHeading > 10.0) {
+        turnRight();
+    } else if(currentDegreeHeading > 180.0 && currentDegreeHeading < 350.0) {
+        turnLeft();
+    } else {
+        goForward();
+    }
+}
+
+int main()
+{
+    //initToF();
+    
+    calibrateIMU();
+    pc.printf("finished imu cal\n");
+
+    zeroDegreeHeading = calculateHeading(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
+                      IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
+    
+    
+    
+    PWM.period(0.001);
+    PWM1.period(0.001);
+    startupESC();
+    pc.printf("finished ecs startup");
+    
+    while(1) {
+        
+        maintainHeading(180.0);
+        
+   /*      
+        char inp = pc.getc();
+        ///// GO FORWARD /////
+        if(inp == 'w' && dutyRight <= 0.230 && dutyLeft <= 0.230) {
+            goForward();
+        }
+        ///// SLOW DOWN /////
+        else if(inp == 's') {
+            slowDown();
+        }
+        else if(inp == 't') {
+            readToF();
+        }
+        else if(inp == 'i') {
+            //readIMU();
+        }
+        ///// TURN LEFT /////
+        else if(inp == 'a' && dutyRight <= 0.230) {
+            turnLeft();
+        }
+        ///// TURN RIGHT /////
+        else if(inp == 'd' && dutyLeft <= 0.230) {
+            turnRight();
+        }
+        ///// STOP /////
+        else if(inp == 'x') {
+            stop();
+        }
+        
+        */
+        ///// STEADY PACE /////
+       //else {
+            
+            ///steady();
+        //}
+    }
+}