MainController.cpp

00001 #include "MainController.h"
00002 
00003 MainController::MainController()
00004     :ch3(p16,0.054,0.092), // frequency
00005      ch4(p17,0.055,0.092), //rudder
00006      ch6(p15,0.055,0.092), //volume
00007      mcon(p22, p6, p7), // change p5 to p7, because p5 is burned through
00008      ap(p25, p26)
00009 {
00010     wait_ms(50);
00011     vol = 0.0;
00012     frq = 10.0;
00013     rud = 0.5;
00014     frqMin = 0.4; // hardcoded
00015     frqMax = 2.5; //hardcoded
00016     goofftime = 0.0;
00017     switched = false;
00018 }
00019 
00020 void MainController::control()
00021 {
00022     curTime = timer1.read();
00023     
00024     if(curTime > 1/frq) {
00025         //read new inputs
00026         vol = this->calculateVolume();
00027         frq = this->calculateFrequency();
00028         rud = this->calculateRudder(); //not used right now
00029         timer1.reset();
00030         curTime = 0.0;
00031         
00032         // rudder value used to define the trapezoid shape
00033         amplitude = vol * ( 2* rud + 1.0);
00034         //amplitude = vol * 3.0;
00035         
00036         switched = false;
00037         goofftime = (vol/(2*frq));
00038             
00039     }
00040        
00041 //    if (curTime > 1/(2*frq) && (switched == false))
00042 //    {
00043 //        switched = true;
00044 //        amplitude = -1.0;
00045 //     }
00046 //        
00047 //    if(!switched)
00048 //    {
00049 //        if(curTime > goofftime )
00050 //        amplitude = 0.0;
00051 //    }
00052 //    else
00053 //    {
00054 //        if(curTime > (1/(2*frq)+goofftime))
00055 //        amplitude = 0.0;
00056 //    }
00057 
00058     dutyCycle = saturate(amplitude * sin( 2.0 * MATH_PI * frq * curTime ));
00059 
00060     mcon.setpolarspeed(dutyCycle);
00061 }
00062 
00063 
00064 float MainController::calculateFrequency()
00065 {
00066     return ((frqMax - frqMin) * ch3.dutycyclescaledup() + frqMin);    
00067 }
00068 
00069 float MainController::calculateVolume()
00070 {
00071     return (ch6.dutycyclescaledup());
00072 }
00073 
00074 float MainController::calculateRudder()
00075 {
00076     return (ch4.dutycyclescaledup());
00077 }
00078 
00079 void MainController::start()
00080 {
00081     timer1.start();
00082     ticker1.attach(this, &MainController::control,0.001);
00083     //Autopilot guardian
00084     //ap.calibrate();
00085     //ap.set2D();
00086     ap.setoff();
00087     
00088 }
00089 
00090 void MainController::stop()
00091 {
00092     timer1.stop();
00093     ticker1.detach();
00094     mcon.setpolarspeed(0.0);
00095 }
00096 
00097 float MainController::getDutyCycle()
00098 {
00099     return dutyCycle;
00100 }
00101 
00102 float MainController::getAmplitude()
00103 {
00104     return amplitude;
00105 }
00106 
00107 
00108 float MainController::getFrequency()
00109 {
00110     return frq;
00111 }
00112 
00113 float MainController::getVolume()
00114 {
00115     return vol;
00116 }
00117 
00118 float MainController::getRudder()
00119 {
00120     return rud;
00121 }
00122 
00123 //signum function
00124 float MainController::signum(float input)
00125 {
00126     if (input>0.0)
00127         return 1.0;
00128     else if (input<0.0)
00129         return (-1.0);
00130     else
00131         return 0.0;
00132 }
00133 
00134 //saturate function
00135 float MainController::saturate(float input)
00136 {
00137     if (input > 1.0)
00138         return (1.0);
00139     else if (input < -1.0)
00140         return (-1.0);
00141     else
00142         return input;
00143 }