the fish that looks like a jet
Dependencies: ADXL345 ADXL345_I2C IMUfilter ITG3200 mbed Servo
Diff: MainController.cpp
- Revision:
- 12:7eeb29892625
- Parent:
- 11:8ec915eb70f6
- Child:
- 13:5ed8fd870723
--- a/MainController.cpp Fri Jan 31 19:36:28 2014 +0000 +++ b/MainController.cpp Sat Feb 01 00:03:40 2014 +0000 @@ -1,15 +1,19 @@ #include "MainController.h" MainController::MainController() - :ch3(p16,0.054,0.092), - ch6(p15,0.055,0.092), - mcon(p22, p6, p5) + :ch3(p16,0.054,0.092), // frequency + ch4(p17,0.055,0.092), //rudder + ch6(p15,0.055,0.092), //volume + mcon(p22, p6, p5), + ap(p25, p26) { - wait_ms(100); + wait_ms(50); vol = 0.0; frq = 10.0; + rud = 0.5; frqMin = 0.5; // hardcoded frqMax = 3.0; //hardcoded + frqmxsqrt = sqrt(frqMax); } void MainController::control() @@ -22,10 +26,15 @@ timer1.reset(); curTime = 0.0; } - amplitude = vol * frq /frqMax; + + rud = this->calculateRudder(); //not used right now + amplitude = vol * sqrt(frq)/frqmxsqrt; dutyCycle = amplitude * sin( 2.0 * MATH_PI * frq * curTime ); + //sigm for alternative driving mode mcon.setpolarspeed(dutyCycle); } + + float MainController::calculateFrequency() { return ((frqMax - frqMin) * ch3.dutycyclescaledup() + frqMin); @@ -36,10 +45,20 @@ return (ch6.dutycyclescaledup()); } +float MainController::calculateRudder() +{ + return (ch4.dutycyclescaledup()); +} + void MainController::start() { timer1.start(); - ticker1.attach(this, &MainController::control,0.0005); + ticker1.attach(this, &MainController::control,0.001); + //Autopilot guardian + //ap.calibrate(); + //ap.set2D(); + ap.setoff(); + } void MainController::stop() @@ -69,3 +88,19 @@ { return vol; } + +float MainController::getRudder() +{ + return rud; +} + +// +//float MainController::sigm(float input) +//{ +// if (input>0) +// return 1; +// else if (input<0) +// return -1; +// else +// return 0; +//} \ No newline at end of file