Working Degree Correction Demo
Dependencies: LSM9DS1_Library_cal TCA9548A VL53L0X mbed mbed-rtos
main.cpp
- Committer:
- jmiller322
- Date:
- 2018-04-24
- Revision:
- 12:505684aec350
- Parent:
- 11:8cd808f5637a
File content as of revision 12:505684aec350:
#include "mbed.h" #include "tca9548a.h" #include "VL53L0X.h" #include "LSM9DS1.h" #include "rtos.h" #define PI 3.14159 #define DECLINATION -4.94 DigitalOut LED(LED1); PwmOut PWM(p21); //Right Side Motor PwmOut PWM1(p22); //Left Side Motor Serial pc(USBTX, USBRX); TCA9548A i2c_sw(I2C_SDA, I2C_SCL); //default address 0x70 applied VL53L0X ToF(I2C_SDA, I2C_SCL); LSM9DS1 IMU(p9, p10, 0xD6, 0x3C); //imu Timer t; Mutex serial_mutex; float dutyLeft=0.130; float dutyRight=0.130; volatile float desHeading = 180.0; float currentDegreeHeading; float distanceAway; volatile bool Phase1=false;//Phase 1 means we are in recieving mode, and should go to the desired heading passed in by the PI volatile bool Phase2=false;//Phase 2 means we are in transmission mode, and should update our desired heading with our current heading. 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"); } LED = 1; IMU.calibrate(1); IMU.calibrateMag(0); LED = 0; return; } void initToF () { // By default TCA9548A performs a power on reset and all downstream ports are deselected for(int i=0;i<4;++i){ //printf(" initializing Tof %d\n",i); i2c_sw.select(i); // select the channel 0 ToF.init(false); //printf("finished initializing Tof %d\n",i); ToF.startContinuous(); //printf("finished starting continous readings for Tof %d\n",i); } i2c_sw.select(0); } void readToF() { printf("Tof 1:%d mm \t",ToF.readRangeContinuousMillimeters()); i2c_sw.select(1); printf("Tof 2:%d mm \t",ToF.readRangeContinuousMillimeters()); i2c_sw.select(2); printf("Tof 3:%d mm \t",ToF.readRangeContinuousMillimeters()); i2c_sw.select(3); printf("Tof 4:%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.148; 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.148; //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.148; dutyLeft = 0.148; 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.05; 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)); currentDegreeHeading=fabs(currentDegreeHeading); serial_mutex.lock(); pc.printf("%d\n",(int)currentDegreeHeading); serial_mutex.unlock(); if(Phase2==true){ stop(); return; } //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 { stop(); } } void serialRx(){ char inp = pc.getc(); ///// GO FORWARD ///// if(inp == 'w') { //goForward(); desHeading += 15.0; } ///// SLOW DOWN ///// else if(inp == 's') { //slowDown(); desHeading -= 15.0; } desHeading = fmod(desHeading, 360); } void init_motors(){ PWM.period(0.001); PWM1.period(0.001); startupESC(); //pc.printf("finished ecs startup"); } void overall_init(){ init_motors(); calibrateIMU(); //printf("finished imu cal\n"); 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)); } void take_input(){ 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(); } } void dev_recv(){ int heading; char headingstr[4]; while(1){ while(pc.readable()) { //pc.scanf("%s", &headingstr); serial_mutex.lock(); pc.gets(headingstr, 4); heading=atoi(headingstr); if(heading==361){ //pc.printf("going into phase 1 \n"); Phase1=true; Phase2=false; heading=0; serial_mutex.unlock(); continue; } if(heading==362){ //pc.printf("going into phase 2 \n"); Phase1=false; Phase2=true; heading=0; serial_mutex.unlock(); continue; } //pc.printf("The recieved heading is %d\n",heading); desHeading=heading; heading=0; serial_mutex.unlock(); } } } void sendHeading(){ while(1){ serial_mutex.lock(); serial_mutex.unlock(); } } int main(){ // pc.printf("before wait function\n"); // wait(10); // pc.printf("before overall init function\n"); // overall_init(); //initToF(); LED = 0; calibrateIMU(); //overallInit(); //pc.printf("before entering the while loop\n"); Thread t1(dev_recv); Thread t2(sendHeading); while(1) { //dev_recv(); //readToF(); maintainHeading(desHeading); //take_input(); } stop(); while(1){ sleep(); } }