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 8:0574a5db1fc4, committed 2014-01-31
- Comitter:
- rkk
- Date:
- Fri Jan 31 04:33:44 2014 +0000
- Parent:
- 7:e005cfaff8d1
- Child:
- 9:8dd7a76756e2
- Commit message:
- updated the motor controller and the pwmin method
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PwmIn.cpp Fri Jan 31 04:33:44 2014 +0000 @@ -0,0 +1,55 @@ +#include "PwmIn.h" + +PwmIn::PwmIn(PinName p, float dutyMin, float dutyMax) + : _p(p) { + _p.rise(this, &PwmIn::rise); + _p.fall(this, &PwmIn::fall); + _period = 20000; + _pulsewidth = 0; + _t.start(); + _risen = false; + _dutyMin = dutyMin; + _dutyMax = dutyMax; + _dutyDelta = dutyMax - dutyMin; + +} + +float PwmIn::period() { + return float(_period)/1000000; +} + +float PwmIn::pulsewidth() { + return float(_pulsewidth)/1000000; +} + +float PwmIn::dutycycle() { + return float(_pulsewidth) / float(_period); +} + +float PwmIn::dutycyclescaledup() { + float duty = float(_pulsewidth) / float(_period); + float dutyAdjusted = (duty > _dutyMin) ? ((duty-_dutyMin)/_dutyDelta) : 0.0; + return (dutyAdjusted < 1.0) ? dutyAdjusted : 1.0; + +} + +void PwmIn::rise() +{ + _tmp = _t.read_us(); + if(_tmp > 19000) { + _period = _tmp; + _risen = true; + _t.reset(); + } +} + +void PwmIn::fall() +{ + _tmp = _t.read_us(); + if(_tmp > 1000 && _tmp < 1950 && _risen == true) { + _pulsewidth = _tmp; + _risen = false; + } +} + + \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PwmIn.h Fri Jan 31 04:33:44 2014 +0000 @@ -0,0 +1,55 @@ + +#ifndef MBED_PWMIN_H +#define MBED_PWMIN_H + +#include "mbed.h" + +/** PwmIn class to read PWM inputs + * + * Uses InterruptIn to measure the changes on the input + * and record the time they occur + * + * @note uses InterruptIn, so not available on p19/p20 + */ +class PwmIn { +public: + /** Create a PwmIn + * + * @param p The pwm input pin (must support InterruptIn) + */ + PwmIn(PinName p, float dutyMin, float dutyMax) ; + + /** Read the current period + * + * @returns the period in seconds + */ + float period(); + + /** Read the current pulsewidth + * + * @returns the pulsewidth in seconds + */ + float pulsewidth(); + + /** Read the current dutycycle + * + * @returns the dutycycle as a percentage, represented between 0.0-1.0 + */ + float dutycycle(); + + float dutycyclescaledup(); + +protected: + void rise(); + void fall(); + + InterruptIn _p; + Timer _t; + int _pulsewidth, _period; + int _tmp; + float _dutyMin, _dutyMax, _dutyDelta; + + bool _risen; +}; + +#endif \ No newline at end of file
--- a/PwmReader.h Thu Jan 30 02:04:23 2014 +0000 +++ b/PwmReader.h Fri Jan 31 04:33:44 2014 +0000 @@ -13,6 +13,8 @@ protected: void pwmRise(); void pwmFall(); + void pwmRise2(); + void pwmFall2(); private: InterruptIn* di;
--- a/main.cpp Thu Jan 30 02:04:23 2014 +0000 +++ b/main.cpp Fri Jan 31 04:33:44 2014 +0000 @@ -4,25 +4,27 @@ //#include "IMU.h" #include "Servo.h" //#include "rtos.h" -#include "PwmReader.h" +//#include "PwmReader.h" +#include "PwmIn.h" -#define THROTTLE_MAX 0.4 -#define THROTTLE_MIN 0.2 +#define VOLUME_MAX 0.4 +#define VOLUME_MIN 0.2 #define FREQUENCY_MAX 0.4 #define FREQUENCY_MIN 0.2 #define RUDDER_MAX 0.4 #define RUDDER_MIN 0.2 +#define SIZE_ARRAY 500 bool quit=false; -InterruptIn event(p16); +//InterruptIn event(p16); PololuMController mcon(p22, p6, p5); -Servo servo(p21); -Guardian ap(p21, p23, p24, p25, p26, p26); -Serial xbee(p13, p14); +//Servo servo(p21); +//Guardian ap(p21, p23, p24, p25, p26, p26); +//Serial xbee(p13, p14); Serial pc(USBTX, USBRX); -PwmReader ch3(p16, 0.054, 0.094); -PwmReader ch6(p15, 0.054, 0.094); +PwmIn ch3(p16,0.054,0.092); +PwmIn ch6(p15,0.055,0.092); Timer t; float throttle, frequency, rudder; @@ -30,19 +32,23 @@ { //ap.calibrate(); //ap.set2D(); - ap.setoff(); + //ap.setoff(); t.start(); - while(1) { + + while(t.read() < 500) { - float vol_norm=ch3.getDuty(); - float freq_norm=ch6.getDuty(); + 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); //pc.printf("time: %f\n\r", t.read()); //pc.printf("throttle: %f frequency: %f rudder: %f\n\r",throttle,frequency,rudder); - //wait_ms(10); + wait_ms(1); } t.stop(); }
--- a/motor_controller.cpp Thu Jan 30 02:04:23 2014 +0000 +++ b/motor_controller.cpp Fri Jan 31 04:33:44 2014 +0000 @@ -18,6 +18,9 @@ outA->write(0); outB->write(1); timestamp=0; + ome1 = 0.0; + amp1 = 0.0; + phi1 = 0.0; } PololuMController::~PololuMController() @@ -57,11 +60,14 @@ return; } -void PololuMController::drive_sinusoidal(float currentTime, float dutyCycle, float frequency) +void PololuMController::drive_sinusoidal(float currentTime, float amplitude, float frequency) { + float ome2 = 2.0* MATH_PI * frequency; + float divisor = (ome2*currentTime); + float phi2 = asin(amp1/amplitude*sin(ome1 * currentTime + phi1))/divisor; - setpolarspeed(dutyCycle*sin( 2.0* MATH_PI* frequency * currentTime)); - return; + setpolarspeed(amplitude*sin(ome2 * currentTime + phi2)); + return; } void PololuMController::drive_rectangular(float currentTime, float amplitude, float frequency)
--- a/motor_controller.h Thu Jan 30 02:04:23 2014 +0000 +++ b/motor_controller.h Fri Jan 31 04:33:44 2014 +0000 @@ -13,6 +13,11 @@ DigitalOut* outA; DigitalOut* outB; float timestamp; + + //for driving + float phi1, ome1,amp1; + + public: PololuMController(); PololuMController(PinName pwmport, PinName A, PinName B); @@ -20,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 dutyCycle, float frequency); + void drive_sinusoidal(float currentTime, float amplitude, float frequency); void drive_rectangular(float currentTime, float amplitude, float frequency); }; \ No newline at end of file