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
Revision 13:5ed8fd870723, committed 2014-02-03
- Comitter:
- rkk
- Date:
- Mon Feb 03 21:04:35 2014 +0000
- Parent:
- 12:7eeb29892625
- Child:
- 14:a5389e26a63b
- Commit message:
- change control approach to have bang bang and impulse length defining
Changed in this revision
MainController.cpp | Show annotated file Show diff for this revision Revisions of this file |
MainController.h | Show annotated file Show diff for this revision Revisions of this file |
--- 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
--- a/MainController.h Sat Feb 01 00:03:40 2014 +0000 +++ b/MainController.h Mon Feb 03 21:04:35 2014 +0000 @@ -46,6 +46,7 @@ float calculateFrequency(); float calculateVolume(); float calculateRudder(); + float signum(float input); private: PwmIn ch3; @@ -65,7 +66,8 @@ float frqMax; float amplitude; float rud; - float frqmxsqrt; + float goofftime; + bool switched; }; #endif