forking so that I can submit a pull request to jmiller322
Dependencies: LSM9DS1_Library_cal TCA9548A VL53L0X mbed
Fork of PWM by
main.cpp
- Committer:
- Joe5181
- Date:
- 2018-04-15
- Revision:
- 3:77b1f376acea
- Parent:
- 2:2a4e77f78c8a
File content as of revision 3:77b1f376acea:
#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); TCA9548A i2c_sw(I2C_SDA, I2C_SCL); //default address 0x70 applied VL53L0X ToF(I2C_SDA, I2C_SCL); LSM9DS1 IMU(p9, p10, 0xD6, 0x3C); //imu float dutyLeft=0.130; float dutyRight=0.130; float desHeading = 180.0; float currentDegreeHeading; float distanceAway; 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 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.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(); } } 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"); desHeading = 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(); } } int main(){ //overall_init(); initToF(); calibrateIMU(); while(1) { readToF(); maintainHeading(desHeading); //take_input(); } } int backup_main(){ }