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
MainController.cpp@11:8ec915eb70f6, 2014-01-31 (annotated)
- Committer:
- rkk
- Date:
- Fri Jan 31 19:36:28 2014 +0000
- Revision:
- 11:8ec915eb70f6
- Child:
- 12:7eeb29892625
added main controller and control amplitude and frequency of fish tail properly
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
rkk | 11:8ec915eb70f6 | 1 | #include "MainController.h" |
rkk | 11:8ec915eb70f6 | 2 | |
rkk | 11:8ec915eb70f6 | 3 | MainController::MainController() |
rkk | 11:8ec915eb70f6 | 4 | :ch3(p16,0.054,0.092), |
rkk | 11:8ec915eb70f6 | 5 | ch6(p15,0.055,0.092), |
rkk | 11:8ec915eb70f6 | 6 | mcon(p22, p6, p5) |
rkk | 11:8ec915eb70f6 | 7 | { |
rkk | 11:8ec915eb70f6 | 8 | wait_ms(100); |
rkk | 11:8ec915eb70f6 | 9 | vol = 0.0; |
rkk | 11:8ec915eb70f6 | 10 | frq = 10.0; |
rkk | 11:8ec915eb70f6 | 11 | frqMin = 0.5; // hardcoded |
rkk | 11:8ec915eb70f6 | 12 | frqMax = 3.0; //hardcoded |
rkk | 11:8ec915eb70f6 | 13 | } |
rkk | 11:8ec915eb70f6 | 14 | |
rkk | 11:8ec915eb70f6 | 15 | void MainController::control() |
rkk | 11:8ec915eb70f6 | 16 | { |
rkk | 11:8ec915eb70f6 | 17 | curTime = timer1.read(); |
rkk | 11:8ec915eb70f6 | 18 | if(curTime > 1/frq) { |
rkk | 11:8ec915eb70f6 | 19 | //read new inputs |
rkk | 11:8ec915eb70f6 | 20 | vol = this->calculateVolume(); |
rkk | 11:8ec915eb70f6 | 21 | frq = this->calculateFrequency(); |
rkk | 11:8ec915eb70f6 | 22 | timer1.reset(); |
rkk | 11:8ec915eb70f6 | 23 | curTime = 0.0; |
rkk | 11:8ec915eb70f6 | 24 | } |
rkk | 11:8ec915eb70f6 | 25 | amplitude = vol * frq /frqMax; |
rkk | 11:8ec915eb70f6 | 26 | dutyCycle = amplitude * sin( 2.0 * MATH_PI * frq * curTime ); |
rkk | 11:8ec915eb70f6 | 27 | mcon.setpolarspeed(dutyCycle); |
rkk | 11:8ec915eb70f6 | 28 | } |
rkk | 11:8ec915eb70f6 | 29 | float MainController::calculateFrequency() |
rkk | 11:8ec915eb70f6 | 30 | { |
rkk | 11:8ec915eb70f6 | 31 | return ((frqMax - frqMin) * ch3.dutycyclescaledup() + frqMin); |
rkk | 11:8ec915eb70f6 | 32 | } |
rkk | 11:8ec915eb70f6 | 33 | |
rkk | 11:8ec915eb70f6 | 34 | float MainController::calculateVolume() |
rkk | 11:8ec915eb70f6 | 35 | { |
rkk | 11:8ec915eb70f6 | 36 | return (ch6.dutycyclescaledup()); |
rkk | 11:8ec915eb70f6 | 37 | } |
rkk | 11:8ec915eb70f6 | 38 | |
rkk | 11:8ec915eb70f6 | 39 | void MainController::start() |
rkk | 11:8ec915eb70f6 | 40 | { |
rkk | 11:8ec915eb70f6 | 41 | timer1.start(); |
rkk | 11:8ec915eb70f6 | 42 | ticker1.attach(this, &MainController::control,0.0005); |
rkk | 11:8ec915eb70f6 | 43 | } |
rkk | 11:8ec915eb70f6 | 44 | |
rkk | 11:8ec915eb70f6 | 45 | void MainController::stop() |
rkk | 11:8ec915eb70f6 | 46 | { |
rkk | 11:8ec915eb70f6 | 47 | timer1.stop(); |
rkk | 11:8ec915eb70f6 | 48 | ticker1.detach(); |
rkk | 11:8ec915eb70f6 | 49 | mcon.setpolarspeed(0.0); |
rkk | 11:8ec915eb70f6 | 50 | } |
rkk | 11:8ec915eb70f6 | 51 | |
rkk | 11:8ec915eb70f6 | 52 | float MainController::getDutyCycle() |
rkk | 11:8ec915eb70f6 | 53 | { |
rkk | 11:8ec915eb70f6 | 54 | return dutyCycle; |
rkk | 11:8ec915eb70f6 | 55 | } |
rkk | 11:8ec915eb70f6 | 56 | |
rkk | 11:8ec915eb70f6 | 57 | float MainController::getAmplitude() |
rkk | 11:8ec915eb70f6 | 58 | { |
rkk | 11:8ec915eb70f6 | 59 | return amplitude; |
rkk | 11:8ec915eb70f6 | 60 | } |
rkk | 11:8ec915eb70f6 | 61 | |
rkk | 11:8ec915eb70f6 | 62 | |
rkk | 11:8ec915eb70f6 | 63 | float MainController::getFrequency() |
rkk | 11:8ec915eb70f6 | 64 | { |
rkk | 11:8ec915eb70f6 | 65 | return frq; |
rkk | 11:8ec915eb70f6 | 66 | } |
rkk | 11:8ec915eb70f6 | 67 | |
rkk | 11:8ec915eb70f6 | 68 | float MainController::getVolume() |
rkk | 11:8ec915eb70f6 | 69 | { |
rkk | 11:8ec915eb70f6 | 70 | return vol; |
rkk | 11:8ec915eb70f6 | 71 | } |