BuoyancyControlUnit.cpp

00001 #include <BuoyancyControlUnit/BuoyancyControlUnit.h>
00002 
00003 // The static instance
00004 BuoyancyControlUnit buoyancyControlUnit;
00005 
00006 //============================================
00007 // Initialization
00008 //============================================
00009 
00010 // Constructor
00011 BuoyancyControlUnit::BuoyancyControlUnit() :
00012                     // Initialize variables
00013                     //bcuPWM(bcuPwmPin),
00014                     bcuDirA(bcuDirAPin),
00015                     bcuDirB(bcuDirBPin),
00016                     //bcuCurrent(bcuCurrentPin),
00017                     wiper(wiperPin),
00018                     pressureSensor(PIN_IMU_TX,PIN_IMU_RX)
00019 {
00020     setDepth = 0; // input command
00021     curDepth = 0; // actual depth based on sensor
00022 
00023     depthLastTime = 0.0;
00024     prevDepthErr = 0.0;
00025     depthInt = 0.0;
00026 
00027     posLastTime = 0.0;
00028     prevPosErr = 0.0;
00029     posInt = 0.0;
00030     VRef = 0;
00031 
00032     resetFlag = 0;
00033     inDepthLoop = 0;
00034     inPosLoop = 0;
00035 }
00036 
00037 
00038 void BuoyancyControlUnit::start()
00039 {
00040     bcuDirA.write(0.0); // apply nothing to start
00041     bcuDirB.write(0.0); // apply nothing to start
00042     //bcuEncoder.reset(); // gives it a reference to return to
00043     
00044     pressureSensor.MS5837Init();
00045     timer.start();
00046     
00047     // pressure sensor takes just over 0.3 seconds to read
00048     depthControl.attach(&buoyancyControlUnit, &BuoyancyControlUnit::setDepthFuncVoid, 0.4);
00049     posControl.attach(&buoyancyControlUnit, &BuoyancyControlUnit::setEncoderPosVoid, 0.1);
00050 }
00051 
00052 void BuoyancyControlUnit::stop()
00053 {
00054     //bcuPWM.write(0.0);
00055     // return bcu back to start position
00056     // have to rethink how to call this because we don't want the while loop stopping everything else
00057 //  while(curPos > 15){
00058 //      this->setEncoderPosition(0);
00059 //  }
00060 
00061     depthControl.detach();
00062     posControl.detach();
00063 
00064     bcuDirA = 0.0;
00065     bcuDirB = 0.0;
00066 }
00067 
00068 void BuoyancyControlUnit::returnToZero()
00069 {
00070     depthControl.detach();
00071     resetFlag = 1;
00072 }
00073 
00074 //============================================
00075 // Processing
00076 //============================================
00077 void BuoyancyControlUnit::set(float depthDesIn)
00078 {
00079     setDepth = depthDesIn;
00080 }
00081 
00082 void BuoyancyControlUnit::setDepthFuncVoid(){
00083     while(inPosLoop);
00084     inDepthLoop = 1;
00085     this->setDepthFunc(setDepth);
00086     inDepthLoop = 0;
00087 }
00088 void BuoyancyControlUnit::setEncoderPosVoid(){
00089     while(inDepthLoop);
00090     inPosLoop = 1;
00091 
00092     if(resetFlag){
00093         posRef = 0;
00094     } else {
00095         depthErr = setDepth - curDepth;
00096         if(depthErr < 5 && depthErr > -5){
00097             posRef = curPos;
00098         }
00099     }
00100 
00101     this->setEncoderPosition(posRef);
00102     inPosLoop = 0;
00103 }
00104 
00105 void BuoyancyControlUnit::setDepthFunc(float depthDesIn){
00106     setDepth = depthDesIn;
00107 //  pressureSensor.Barometer_MS5837();
00108 //  curDepth = pressureSensor.MS5837_Pressure();
00109 /*
00110 TODO: Map measured depth to 0 - 30 using h = P/rg. 
00111 #define MIN_DEPTH 1019.0 //mbar
00112 curDepth = curDepth * (fishMaxPitch - fishMinPitch)/(MAX_DEPTH - MIN_DEPTH)
00113 */
00114     curDepth = (curPos * 1.25); // fake depth readings
00115 
00116     depthErr = setDepth - curDepth;
00117 
00118 /*  float depthdt = timer.read() - depthLastTime;
00119 
00120     depthInt += depthErr * depthdt;
00121     depthDer = (depthErr - prevDepthErr)/depthdt;
00122 
00123     posRef += KpDepth*depthErr + KiDepth*depthInt + KdDepth*depthDer;
00124 */
00125     posRef += KpDepth * depthErr;
00126     
00127     if (posRef > 30){
00128         posRef = 30;
00129     }
00130     else if (posRef < 0){
00131         posRef = 0;
00132     }
00133 
00134 //  if(posRef < 0){
00135 //      posRef = 0; // limit position to only values greater than 0
00136 //      depthInt = 0; // reset these values so they don't build while the bcu can't reach this depth value
00137 //      depthDer = 0;
00138 //  }
00139 
00140 //  depthLastTime = timer.read();
00141 //  prevDepthErr = depthErr;
00142 }
00143 
00144 
00145 void BuoyancyControlUnit::setEncoderPosition(float setPosIn) {
00146 
00147     setPos = setPosIn;
00148     if(setPos < 0){ setPos = 0; }
00149     curPos = wiper * (30.0); //map ADC reading to system level voltage, try adjusting pitch max and min values (0 - 30)
00150     posErr = setPos - curPos;
00151     float posdt = timer.read() - posLastTime;
00152 
00153     posInt += posErr * posdt;
00154     posDer = (posErr - prevPosErr)/posdt;
00155 
00156     VRef = KpEnc * posErr + KiEnc * posInt + KdEnc * posDer;
00157 //  VRef = KpEnc * posErr;
00158 
00159     /* Add constrain + map here */
00160     if (VRef > 6.0){
00161         VRef = 6.0;
00162     }
00163     else if (VRef < -6.0){
00164         VRef = -6.0;
00165     }
00166     
00167     // Map Vref to 0.7 - 1.0 duty cycle
00168     if (VRef < 0){
00169         VRef = VRef * (0.3/(-6.0)) + 0.7;
00170         bcuDir = 1;
00171     }
00172     else if (VRef > 0){
00173         VRef = VRef * (0.3/(6.0)) + 0.7;
00174         bcuDir = 0;
00175     }
00176     
00177     posLastTime = timer.read();
00178     prevPosErr = posErr;
00179 
00180     if(resetFlag){
00181         if(posErr > 10 || posErr < -10){
00182             this->setV(VRef);
00183         } else {
00184             resetFlag = 0;
00185             this->stop();
00186         }
00187     } else {
00188         if(posErr > 5 || posErr < -5) { // TODO: determine appropriate tolerance (final error from target pos?)
00189             this->setV(VRef);
00190             #ifdef debugBCU
00191             DigitalOut test(LED1);
00192             test = 0;
00193             #endif
00194         } else {
00195             bcuDirA = 0.0;
00196             bcuDirB = 0;
00197             #ifdef debugBCU
00198             //Testing whether fishController is running
00199             DigitalOut test(LED1);
00200             test = 1;
00201             #endif 
00202         }
00203     }
00204 }
00205 
00206 void BuoyancyControlUnit::setV(float Vin){
00207     float setV;
00208     
00209     if(bcuDir == 1){
00210         bcuDirA.write(setV);
00211         bcuDirB.write(0.0);
00212     } else if(bcuDir == 0){
00213         bcuDirA.write(0.0);
00214         bcuDirB.write(setV);
00215     }
00216     
00217 }
00218 
00219 void BuoyancyControlUnit::runBackwards() {
00220     this->stop();
00221     this->setV(-0.2);
00222 }
00223 
00224 void BuoyancyControlUnit::runForwards() {
00225     this->stop();
00226     this->setV(0.2);
00227 }
00228 
00229 bool BuoyancyControlUnit::getBCUdir() { return bcuDir; }
00230 float BuoyancyControlUnit::getVset() { return VRef; }
00231 float BuoyancyControlUnit::getSetDepth() { return setDepth; }
00232 float BuoyancyControlUnit::getCurDepth() { return curDepth; }
00233 float BuoyancyControlUnit::getSetPos() { return setPos; }
00234 float BuoyancyControlUnit::getCurPos() { return curPos; }
00235 float BuoyancyControlUnit::readPressure()
00236 {
00237     pressureSensor.Barometer_MS5837();
00238     return pressureSensor.MS5837_Pressure();
00239 }