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 10:d9f1037f0cb0, committed 2014-01-31
- Comitter:
- rkk
- Date:
- Fri Jan 31 05:18:19 2014 +0000
- Parent:
- 9:8dd7a76756e2
- Child:
- 11:8ec915eb70f6
- Commit message:
- still working on driving mode
Changed in this revision
--- a/main.cpp Fri Jan 31 04:53:10 2014 +0000 +++ b/main.cpp Fri Jan 31 05:18:19 2014 +0000 @@ -39,13 +39,12 @@ float vol_norm=ch6.dutycyclescaledup(); float freq_norm=ch3.dutycyclescaledup(); - - - pc.printf("channel 3: %f, channel 6: %f\n", vol_norm, freq_norm); + //mcon.drive_rectangular(t.read(), vol_norm*freq_norm, 3*freq_norm); //mcon.drive_sinusoidal(t.read(), vol_norm*freq_norm, 3*freq_norm); - mcon.drive_sinusoidal(t.read(), vol_norm*freq_norm, 3*freq_norm); + float dutycycle = mcon.drive_sinusoidal(t.read(), vol_norm*freq_norm, freq_norm); + pc.printf("ch 3: %f, ch 6: %f, dc: %f, asin: %f\n", vol_norm, freq_norm, dutycycle, asin(sin(t.read())) ); //pc.printf("time: %f\n\r", t.read()); //pc.printf("throttle: %f frequency: %f rudder: %f\n\r",throttle,frequency,rudder); wait_ms(20);
--- 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)
--- a/motor_controller.h Fri Jan 31 04:53:10 2014 +0000 +++ b/motor_controller.h Fri Jan 31 05:18:19 2014 +0000 @@ -16,7 +16,7 @@ //for driving float phi1, ome1,amp1; - + bool firstTime; public: PololuMController(); @@ -25,6 +25,6 @@ void setspeed(float speed); //0 to 1 void setpolarspeed(float speed); //-1 to 1 void reverse(); //only works on non-polar speed - void drive_sinusoidal(float currentTime, float amplitude, float frequency); + float drive_sinusoidal(float currentTime, float amplitude, float frequency); void drive_rectangular(float currentTime, float amplitude, float frequency); }; \ No newline at end of file