the fish that looks like a jet
Dependencies: ADXL345 ADXL345_I2C IMUfilter ITG3200 mbed Servo
Diff: motor_controller.cpp
- Revision:
- 10:d9f1037f0cb0
- Parent:
- 9:8dd7a76756e2
- Child:
- 11:8ec915eb70f6
diff -r 8dd7a76756e2 -r d9f1037f0cb0 motor_controller.cpp --- a/motor_controller.cpp Fri Jan 31 04:53:10 2014 +0000 +++ b/motor_controller.cpp Fri Jan 31 05:18:19 2014 +0000 @@ -21,6 +21,7 @@ ome1 = 0.0; amp1 = 0.0; phi1 = 0.0; + firstTime = true; } PololuMController::~PololuMController() @@ -60,29 +61,41 @@ return; } -void PololuMController::drive_sinusoidal(float currentTime, float amplitude, float frequency) +float PololuMController::drive_sinusoidal(float currentTime, float amplitude, float frequency) { - float ome2 = 2.0* MATH_PI * frequency; - float divisor = (ome2*currentTime); - 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); - - //set previous values - ome1 = ome2; - - amp1 = amplitude; - - return; + + //convert inputs + float amp2; + if(amplitude > 1.0) { + amp2 = 1.0; + } else if(amplitude < 0.0) { + amp2 = 0.0; + } else { + amp2 = amplitude; + } + float ome2 = 2.0* MATH_PI * frequency; + + float dutycycle; + float phi2; + if (firstTime) + { + dutycycle = amp2*sin(ome2 * currentTime); + firstTime = false; + } + else if(amp2 > 0.0) { + phi2 = asin(amp1 * sin(ome1 * currentTime + phi1) / amp2 ) - (ome2*currentTime); + dutycycle = amp2*sin(ome2 * currentTime + phi2); + phi1 = phi2; + } else { + dutycycle = 0.0; + } + setpolarspeed(dutycycle); + + //set previous values + ome1 = ome2; + amp1 = amp2; + + return dutycycle; } void PololuMController::drive_rectangular(float currentTime, float amplitude, float frequency)