the fish that looks like a jet
Dependencies: ADXL345 ADXL345_I2C IMUfilter ITG3200 mbed Servo
Diff: motor_controller.cpp
- Revision:
- 9:8dd7a76756e2
- Parent:
- 8:0574a5db1fc4
- Child:
- 10:d9f1037f0cb0
diff -r 0574a5db1fc4 -r 8dd7a76756e2 motor_controller.cpp --- 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; }