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: motor_controller.cpp
- Revision:
- 9:8dd7a76756e2
- Parent:
- 8:0574a5db1fc4
- Child:
- 10:d9f1037f0cb0
--- a/motor_controller.cpp Fri Jan 31 04:33:44 2014 +0000 +++ b/motor_controller.cpp Fri Jan 31 04:53:10 2014 +0000 @@ -64,9 +64,24 @@ { float ome2 = 2.0* MATH_PI * frequency; float divisor = (ome2*currentTime); - float phi2 = asin(amp1/amplitude*sin(ome1 * currentTime + phi1))/divisor; + float dutycycle; + float phi2; + if(divisor > 0.0 && amplitude > 0.0) + { + phi2 = asin(amp1/amplitude*sin(ome1 * currentTime + phi1))/divisor; + dutycycle = amplitude*sin(ome2 * currentTime + phi2); + phi1 = phi2; + } + else{ + dutycycle = 0.0; + } + setpolarspeed(dutycycle); - setpolarspeed(amplitude*sin(ome2 * currentTime + phi2)); + //set previous values + ome1 = ome2; + + amp1 = amplitude; + return; }