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:
- 15:dc5753a5b83e
- Parent:
- 14:a5389e26a63b
- Child:
- 16:79cfe6201318
diff -r a5389e26a63b -r dc5753a5b83e MainController.cpp --- a/MainController.cpp Tue Feb 04 14:23:56 2014 +0000 +++ b/MainController.cpp Wed Feb 12 23:57:01 2014 +0000 @@ -11,8 +11,8 @@ vol = 0.0; frq = 10.0; rud = 0.5; - frqMin = 0.5; // hardcoded - frqMax = 3.0; //hardcoded + frqMin = 0.4; // hardcoded + frqMax = 2.5; //hardcoded goofftime = 0.0; switched = false; } @@ -20,6 +20,7 @@ void MainController::control() { curTime = timer1.read(); + if(curTime > 1/frq) { //read new inputs vol = this->calculateVolume(); @@ -27,14 +28,16 @@ rud = this->calculateRudder(); //not used right now timer1.reset(); curTime = 0.0; + + // rudder value used to define the trapezoid shape + amplitude = vol * ( 2* rud + 1.0); + //amplitude = vol * 3.0; + switched = false; goofftime = (vol/(2*frq)); - amplitude = vol; - + } - - //vol * sqrt(frq)/frqmxsqrt; - + // if (curTime > 1/(2*frq) && (switched == false)) // { // switched = true; @@ -52,7 +55,7 @@ // amplitude = 0.0; // } - dutyCycle = amplitude * signum(sin( 2.0 * MATH_PI * frq * curTime )); + dutyCycle = saturate(amplitude * sin( 2.0 * MATH_PI * frq * curTime )); mcon.setpolarspeed(dutyCycle); } @@ -117,7 +120,7 @@ return rud; } -// +//signum function float MainController::signum(float input) { if (input>0.0) @@ -126,4 +129,15 @@ return (-1.0); else return 0.0; +} + +//saturate function +float MainController::saturate(float input) +{ + if (input > 1.0) + return (1.0); + else if (input < -1.0) + return (-1.0); + else + return input; } \ No newline at end of file