the fish that looks like a jet
Dependencies: ADXL345 ADXL345_I2C IMUfilter ITG3200 mbed Servo
Revision 11:8ec915eb70f6, committed 2014-01-31
- Comitter:
- rkk
- Date:
- Fri Jan 31 19:36:28 2014 +0000
- Parent:
- 10:d9f1037f0cb0
- Child:
- 12:7eeb29892625
- Commit message:
- added main controller and control amplitude and frequency of fish tail properly
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MainController.cpp Fri Jan 31 19:36:28 2014 +0000 @@ -0,0 +1,71 @@ +#include "MainController.h" + +MainController::MainController() + :ch3(p16,0.054,0.092), + ch6(p15,0.055,0.092), + mcon(p22, p6, p5) +{ + wait_ms(100); + vol = 0.0; + frq = 10.0; + frqMin = 0.5; // hardcoded + frqMax = 3.0; //hardcoded +} + +void MainController::control() +{ + curTime = timer1.read(); + if(curTime > 1/frq) { + //read new inputs + vol = this->calculateVolume(); + frq = this->calculateFrequency(); + timer1.reset(); + curTime = 0.0; + } + amplitude = vol * frq /frqMax; + dutyCycle = amplitude * sin( 2.0 * MATH_PI * frq * curTime ); + mcon.setpolarspeed(dutyCycle); +} +float MainController::calculateFrequency() +{ + return ((frqMax - frqMin) * ch3.dutycyclescaledup() + frqMin); +} + +float MainController::calculateVolume() +{ + return (ch6.dutycyclescaledup()); +} + +void MainController::start() +{ + timer1.start(); + ticker1.attach(this, &MainController::control,0.0005); +} + +void MainController::stop() +{ + timer1.stop(); + ticker1.detach(); + mcon.setpolarspeed(0.0); +} + +float MainController::getDutyCycle() +{ + return dutyCycle; +} + +float MainController::getAmplitude() +{ + return amplitude; +} + + +float MainController::getFrequency() +{ + return frq; +} + +float MainController::getVolume() +{ + return vol; +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MainController.h Fri Jan 31 19:36:28 2014 +0000 @@ -0,0 +1,62 @@ +#ifndef MBED_MAINCONTROLLER_H +#define MBED_MAINCONTROLLER_H + +#include "mbed.h" +#include "PwmIn.h" +#include "motor_controller.h" + +#define MATH_PI 3.14159265359 + +/** MainController class to get control inputs and place them onto the system + * + * + */ +class MainController { +public: + /** Create a MainController + * + * @param + */ + MainController() ; + + /** Start the main controller + * + * @returns + */ + void start(); + + float getDutyCycle(); + float getFrequency(); + float getVolume(); + float getAmplitude(); + + /** Stop the main controller + * + * @returns + */ + void stop(); + + +protected: + void control(); + float calculateFrequency(); + float calculateVolume(); + +private: + PwmIn ch3; + PwmIn ch6; + PololuMController mcon; + Timer timer1; + Ticker ticker1; + float vol; + float frq; + float dutyCycle; + float curTime; + float frqMin; + float frqMax; + float amplitude; +}; + +#endif + +
--- a/main.cpp Fri Jan 31 05:18:19 2014 +0000 +++ b/main.cpp Fri Jan 31 19:36:28 2014 +0000 @@ -1,33 +1,54 @@ #include "mbed.h" -#include "motor_controller.h" +//#include "motor_controller.h" #include "guardian.h" //#include "IMU.h" #include "Servo.h" //#include "rtos.h" //#include "PwmReader.h" -#include "PwmIn.h" +//#include "PwmIn.h" +#include "MainController.h" -#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; +//bool quit=false; //InterruptIn event(p16); -PololuMController mcon(p22, p6, p5); + //Servo servo(p21); //Guardian ap(p21, p23, p24, p25, p26, p26); //Serial xbee(p13, p14); Serial pc(USBTX, USBRX); -PwmIn ch3(p16,0.054,0.092); -PwmIn ch6(p15,0.055,0.092); +// Timer t; +// +// +//int counter; +//int divisions; +//float vol,frq; +//PwmOut led(LED1); -float throttle, frequency, rudder; +//void dosomething () +//{ +// if( counter == divisions) { +// ticker1.detach(); +// vol = ch6.dutycyclescaledup(); +// frq = ch3.dutycyclescaledup(); +// counter = 0; +// ticker1.attach(&dosomething,1/(frq*float(divisions))); +// } +// +// +// +// } +// +// +//void startsomething() +//{ +// counter = 0; +// divisions = 20; +// +// +// ticker1.attach(&dosomething,1/(frq*float(divisions))); +//} + int main() { //ap.calibrate(); @@ -35,19 +56,25 @@ //ap.setoff(); t.start(); + MainController mainCtrl; + + + //startsomething(); + + mainCtrl.start(); + + while(t.read() < 500) { + - float vol_norm=ch6.dutycyclescaledup(); - float freq_norm=ch3.dutycyclescaledup(); - - //mcon.drive_rectangular(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())) ); + //float dutycycle = mcon.drive_sinusoidal(t.read(), vol_norm*freq_norm, freq_norm); + pc.printf("frq: %f, vol: %f, amp: %f, dut: %f\n", mainCtrl.getFrequency(), mainCtrl.getVolume(), mainCtrl.getAmplitude(),mainCtrl.getDutyCycle() ); //pc.printf("time: %f\n\r", t.read()); //pc.printf("throttle: %f frequency: %f rudder: %f\n\r",throttle,frequency,rudder); - wait_ms(20); + wait_ms(100); } t.stop(); + mainCtrl.stop(); }
--- a/motor_controller.cpp Fri Jan 31 05:18:19 2014 +0000 +++ b/motor_controller.cpp Fri Jan 31 19:36:28 2014 +0000 @@ -1,109 +1,101 @@ #include "motor_controller.h" - -float sigm(float input) -{ - if (input>0) - return 1; - else if (input<0) - return -1; - else - return 0; -} +// +//float sigm(float input) +//{ +// if (input>0) +// return 1; +// else if (input<0) +// return -1; +// else +// return 0; +//} PololuMController::PololuMController(PinName pwmport, PinName A, PinName B) -{ - pwm=new PwmOut(pwmport); - outA=new DigitalOut(A); - outB=new DigitalOut(B); - outA->write(0); - outB->write(1); - timestamp=0; - ome1 = 0.0; - amp1 = 0.0; - phi1 = 0.0; - firstTime = true; +:pwm(pwmport),outA(A),outB(B){ + outA.write(0); + outB.write(1); +// timestamp=0; +// ome1 = 0.0; +// amp1 = 0.0; +// phi1 = 0.0; +// firstTime = true; + } -PololuMController::~PololuMController() -{ - delete pwm; - delete outA; - delete outB; -} -void PololuMController::setspeed(float speed) -{ - pwm->write(speed); - return; -} +//void PololuMController::setspeed(float speed) +//{ +// pwm->write(speed); +// return; +//} void PololuMController::setpolarspeed(float speed) { if (speed>=0) { - outA->write(0); - outB->write(1); - pwm->write(abs(speed)); + outA.write(0); + outB.write(1); + pwm.write(abs(speed)); } else { - outA->write(1); - outB->write(0); - pwm->write(abs(speed)); + outA.write(1); + outB.write(0); + pwm.write(abs(speed)); } return; } -void PololuMController::reverse() -{ - outA->write(!(outA->read())); - outB->write(!(outB->read())); - return; -} - -float PololuMController::drive_sinusoidal(float currentTime, float amplitude, float frequency) -{ - - //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; +//void PololuMController::reverse() +//{ +// outA->write(!(outA->read())); +// outB->write(!(outB->read())); +// return; +//} - 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) -{ - //setpolarspeed(dutyCycle*sin( 2.0* MATH_PI* f * currentTime)); - float sinRes = sin( 2.0* MATH_PI* frequency * currentTime); - - float dutycycle = amplitude * sigm( sinRes); - setpolarspeed(dutycycle); - return; -} +//float PololuMController::drive_sinusoidal(float currentTime, float amplitude, float frequency) +//{ +// +// //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) +//{ +// //setpolarspeed(dutyCycle*sin( 2.0* MATH_PI* f * currentTime)); +// float sinRes = sin( 2.0* MATH_PI* frequency * currentTime); +// +// float dutycycle = amplitude * sigm( sinRes); +// setpolarspeed(dutycycle); +// return; +//}
--- a/motor_controller.h Fri Jan 31 05:18:19 2014 +0000 +++ b/motor_controller.h Fri Jan 31 19:36:28 2014 +0000 @@ -1,6 +1,6 @@ #pragma once #include "mbed.h" -#define MATH_PI 3.14159265359 + #define FREQ_MIN 0.5 //Hz #define FREQ_MAX 3 //Hz @@ -9,22 +9,24 @@ class PololuMController { private: - PwmOut* pwm; - DigitalOut* outA; - DigitalOut* outB; - float timestamp; + PwmOut pwm; + DigitalOut outA; + DigitalOut outB; + //float timestamp; - //for driving - float phi1, ome1,amp1; - bool firstTime; +// //for driving +// float phi1, ome1,amp1; +// bool firstTime; +// +// Timer t; + public: PololuMController(); PololuMController(PinName pwmport, PinName A, PinName B); - ~PololuMController(); - void setspeed(float speed); //0 to 1 + //void setspeed(float speed); //0 to 1 void setpolarspeed(float speed); //-1 to 1 - void reverse(); //only works on non-polar speed - float drive_sinusoidal(float currentTime, float amplitude, float frequency); - void drive_rectangular(float currentTime, float amplitude, float frequency); + //void reverse(); //only works on non-polar speed + //float drive_sinusoidal(float currentTime, float amplitude, float frequency); + //void drive_rectangular(float currentTime, float amplitude, float frequency); }; \ No newline at end of file