My modifications/additions to the code
Dependencies: ADXL345 ADXL345_I2C IMUfilter ITG3200 Servo fishgait mbed-rtos mbed pixy_cam
Fork of robotic_fish_ver_4_8 by
Diff: MainController.cpp
- Revision:
- 13:5ed8fd870723
- Parent:
- 12:7eeb29892625
- Child:
- 14:a5389e26a63b
--- a/MainController.cpp Sat Feb 01 00:03:40 2014 +0000 +++ b/MainController.cpp Mon Feb 03 21:04:35 2014 +0000 @@ -13,7 +13,8 @@ rud = 0.5; frqMin = 0.5; // hardcoded frqMax = 3.0; //hardcoded - frqmxsqrt = sqrt(frqMax); + goofftime = 0.0; + switched = false; } void MainController::control() @@ -23,14 +24,36 @@ //read new inputs vol = this->calculateVolume(); frq = this->calculateFrequency(); + rud = this->calculateRudder(); //not used right now timer1.reset(); curTime = 0.0; + switched = false; + goofftime = (1/(2*frq)*vol); + amplitude = 1.0; + } - 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 + //vol * sqrt(frq)/frqmxsqrt; + + if (curTime > 1/(2*frq)) + { + switched = true; + amplitude = -1.0; + } + + if(!switched) + { + if(curTime > goofftime ) + amplitude = 0.0; + } + else + { + if(curTime > (1/(2*frq)+goofftime)) + amplitude = 0.0; + } + + dutyCycle = amplitude * signum(sin( 2.0 * MATH_PI * frq * curTime )); + mcon.setpolarspeed(dutyCycle); } @@ -95,12 +118,12 @@ } // -//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 +float MainController::signum(float input) +{ + if (input>0.0) + return 1; + else if (input<0.0) + return -1; + else + return 0; +} \ No newline at end of file