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@15:dc5753a5b83e, 2014-02-12 (annotated)
- Committer:
- rkk
- Date:
- Wed Feb 12 23:57:01 2014 +0000
- Revision:
- 15:dc5753a5b83e
- Parent:
- 14:a5389e26a63b
- Child:
- 16:79cfe6201318
added the ability to use the rudder to control the amplitude
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 | 12:7eeb29892625 | 4 | :ch3(p16,0.054,0.092), // frequency |
rkk | 12:7eeb29892625 | 5 | ch4(p17,0.055,0.092), //rudder |
rkk | 12:7eeb29892625 | 6 | ch6(p15,0.055,0.092), //volume |
rkk | 12:7eeb29892625 | 7 | mcon(p22, p6, p5), |
rkk | 12:7eeb29892625 | 8 | ap(p25, p26) |
rkk | 11:8ec915eb70f6 | 9 | { |
rkk | 12:7eeb29892625 | 10 | wait_ms(50); |
rkk | 11:8ec915eb70f6 | 11 | vol = 0.0; |
rkk | 11:8ec915eb70f6 | 12 | frq = 10.0; |
rkk | 12:7eeb29892625 | 13 | rud = 0.5; |
rkk | 15:dc5753a5b83e | 14 | frqMin = 0.4; // hardcoded |
rkk | 15:dc5753a5b83e | 15 | frqMax = 2.5; //hardcoded |
rkk | 13:5ed8fd870723 | 16 | goofftime = 0.0; |
rkk | 13:5ed8fd870723 | 17 | switched = false; |
rkk | 11:8ec915eb70f6 | 18 | } |
rkk | 11:8ec915eb70f6 | 19 | |
rkk | 11:8ec915eb70f6 | 20 | void MainController::control() |
rkk | 11:8ec915eb70f6 | 21 | { |
rkk | 11:8ec915eb70f6 | 22 | curTime = timer1.read(); |
rkk | 15:dc5753a5b83e | 23 | |
rkk | 11:8ec915eb70f6 | 24 | if(curTime > 1/frq) { |
rkk | 11:8ec915eb70f6 | 25 | //read new inputs |
rkk | 11:8ec915eb70f6 | 26 | vol = this->calculateVolume(); |
rkk | 11:8ec915eb70f6 | 27 | frq = this->calculateFrequency(); |
rkk | 13:5ed8fd870723 | 28 | rud = this->calculateRudder(); //not used right now |
rkk | 11:8ec915eb70f6 | 29 | timer1.reset(); |
rkk | 11:8ec915eb70f6 | 30 | curTime = 0.0; |
rkk | 15:dc5753a5b83e | 31 | |
rkk | 15:dc5753a5b83e | 32 | // rudder value used to define the trapezoid shape |
rkk | 15:dc5753a5b83e | 33 | amplitude = vol * ( 2* rud + 1.0); |
rkk | 15:dc5753a5b83e | 34 | //amplitude = vol * 3.0; |
rkk | 15:dc5753a5b83e | 35 | |
rkk | 13:5ed8fd870723 | 36 | switched = false; |
rkk | 14:a5389e26a63b | 37 | goofftime = (vol/(2*frq)); |
rkk | 15:dc5753a5b83e | 38 | |
rkk | 11:8ec915eb70f6 | 39 | } |
rkk | 15:dc5753a5b83e | 40 | |
rkk | 14:a5389e26a63b | 41 | // if (curTime > 1/(2*frq) && (switched == false)) |
rkk | 14:a5389e26a63b | 42 | // { |
rkk | 14:a5389e26a63b | 43 | // switched = true; |
rkk | 14:a5389e26a63b | 44 | // amplitude = -1.0; |
rkk | 14:a5389e26a63b | 45 | // } |
rkk | 14:a5389e26a63b | 46 | // |
rkk | 14:a5389e26a63b | 47 | // if(!switched) |
rkk | 14:a5389e26a63b | 48 | // { |
rkk | 14:a5389e26a63b | 49 | // if(curTime > goofftime ) |
rkk | 14:a5389e26a63b | 50 | // amplitude = 0.0; |
rkk | 14:a5389e26a63b | 51 | // } |
rkk | 14:a5389e26a63b | 52 | // else |
rkk | 14:a5389e26a63b | 53 | // { |
rkk | 14:a5389e26a63b | 54 | // if(curTime > (1/(2*frq)+goofftime)) |
rkk | 14:a5389e26a63b | 55 | // amplitude = 0.0; |
rkk | 14:a5389e26a63b | 56 | // } |
rkk | 13:5ed8fd870723 | 57 | |
rkk | 15:dc5753a5b83e | 58 | dutyCycle = saturate(amplitude * sin( 2.0 * MATH_PI * frq * curTime )); |
rkk | 13:5ed8fd870723 | 59 | |
rkk | 11:8ec915eb70f6 | 60 | mcon.setpolarspeed(dutyCycle); |
rkk | 11:8ec915eb70f6 | 61 | } |
rkk | 12:7eeb29892625 | 62 | |
rkk | 12:7eeb29892625 | 63 | |
rkk | 11:8ec915eb70f6 | 64 | float MainController::calculateFrequency() |
rkk | 11:8ec915eb70f6 | 65 | { |
rkk | 11:8ec915eb70f6 | 66 | return ((frqMax - frqMin) * ch3.dutycyclescaledup() + frqMin); |
rkk | 11:8ec915eb70f6 | 67 | } |
rkk | 11:8ec915eb70f6 | 68 | |
rkk | 11:8ec915eb70f6 | 69 | float MainController::calculateVolume() |
rkk | 11:8ec915eb70f6 | 70 | { |
rkk | 11:8ec915eb70f6 | 71 | return (ch6.dutycyclescaledup()); |
rkk | 11:8ec915eb70f6 | 72 | } |
rkk | 11:8ec915eb70f6 | 73 | |
rkk | 12:7eeb29892625 | 74 | float MainController::calculateRudder() |
rkk | 12:7eeb29892625 | 75 | { |
rkk | 12:7eeb29892625 | 76 | return (ch4.dutycyclescaledup()); |
rkk | 12:7eeb29892625 | 77 | } |
rkk | 12:7eeb29892625 | 78 | |
rkk | 11:8ec915eb70f6 | 79 | void MainController::start() |
rkk | 11:8ec915eb70f6 | 80 | { |
rkk | 11:8ec915eb70f6 | 81 | timer1.start(); |
rkk | 12:7eeb29892625 | 82 | ticker1.attach(this, &MainController::control,0.001); |
rkk | 12:7eeb29892625 | 83 | //Autopilot guardian |
rkk | 12:7eeb29892625 | 84 | //ap.calibrate(); |
rkk | 12:7eeb29892625 | 85 | //ap.set2D(); |
rkk | 12:7eeb29892625 | 86 | ap.setoff(); |
rkk | 12:7eeb29892625 | 87 | |
rkk | 11:8ec915eb70f6 | 88 | } |
rkk | 11:8ec915eb70f6 | 89 | |
rkk | 11:8ec915eb70f6 | 90 | void MainController::stop() |
rkk | 11:8ec915eb70f6 | 91 | { |
rkk | 11:8ec915eb70f6 | 92 | timer1.stop(); |
rkk | 11:8ec915eb70f6 | 93 | ticker1.detach(); |
rkk | 11:8ec915eb70f6 | 94 | mcon.setpolarspeed(0.0); |
rkk | 11:8ec915eb70f6 | 95 | } |
rkk | 11:8ec915eb70f6 | 96 | |
rkk | 11:8ec915eb70f6 | 97 | float MainController::getDutyCycle() |
rkk | 11:8ec915eb70f6 | 98 | { |
rkk | 11:8ec915eb70f6 | 99 | return dutyCycle; |
rkk | 11:8ec915eb70f6 | 100 | } |
rkk | 11:8ec915eb70f6 | 101 | |
rkk | 11:8ec915eb70f6 | 102 | float MainController::getAmplitude() |
rkk | 11:8ec915eb70f6 | 103 | { |
rkk | 11:8ec915eb70f6 | 104 | return amplitude; |
rkk | 11:8ec915eb70f6 | 105 | } |
rkk | 11:8ec915eb70f6 | 106 | |
rkk | 11:8ec915eb70f6 | 107 | |
rkk | 11:8ec915eb70f6 | 108 | float MainController::getFrequency() |
rkk | 11:8ec915eb70f6 | 109 | { |
rkk | 11:8ec915eb70f6 | 110 | return frq; |
rkk | 11:8ec915eb70f6 | 111 | } |
rkk | 11:8ec915eb70f6 | 112 | |
rkk | 11:8ec915eb70f6 | 113 | float MainController::getVolume() |
rkk | 11:8ec915eb70f6 | 114 | { |
rkk | 11:8ec915eb70f6 | 115 | return vol; |
rkk | 11:8ec915eb70f6 | 116 | } |
rkk | 12:7eeb29892625 | 117 | |
rkk | 12:7eeb29892625 | 118 | float MainController::getRudder() |
rkk | 12:7eeb29892625 | 119 | { |
rkk | 12:7eeb29892625 | 120 | return rud; |
rkk | 12:7eeb29892625 | 121 | } |
rkk | 12:7eeb29892625 | 122 | |
rkk | 15:dc5753a5b83e | 123 | //signum function |
rkk | 13:5ed8fd870723 | 124 | float MainController::signum(float input) |
rkk | 13:5ed8fd870723 | 125 | { |
rkk | 13:5ed8fd870723 | 126 | if (input>0.0) |
rkk | 14:a5389e26a63b | 127 | return 1.0; |
rkk | 13:5ed8fd870723 | 128 | else if (input<0.0) |
rkk | 14:a5389e26a63b | 129 | return (-1.0); |
rkk | 13:5ed8fd870723 | 130 | else |
rkk | 14:a5389e26a63b | 131 | return 0.0; |
rkk | 15:dc5753a5b83e | 132 | } |
rkk | 15:dc5753a5b83e | 133 | |
rkk | 15:dc5753a5b83e | 134 | //saturate function |
rkk | 15:dc5753a5b83e | 135 | float MainController::saturate(float input) |
rkk | 15:dc5753a5b83e | 136 | { |
rkk | 15:dc5753a5b83e | 137 | if (input > 1.0) |
rkk | 15:dc5753a5b83e | 138 | return (1.0); |
rkk | 15:dc5753a5b83e | 139 | else if (input < -1.0) |
rkk | 15:dc5753a5b83e | 140 | return (-1.0); |
rkk | 15:dc5753a5b83e | 141 | else |
rkk | 15:dc5753a5b83e | 142 | return input; |
rkk | 13:5ed8fd870723 | 143 | } |