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@21:835fd919b4bd, 2014-06-03 (annotated)
- Committer:
- sandwich
- Date:
- Tue Jun 03 15:30:09 2014 +0000
- Revision:
- 21:835fd919b4bd
- Parent:
- 20:6ae16da1492a
- Child:
- 22:807d5467fbf6
incorporating pixy
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 | 20:6ae16da1492a | 4 | :ch1(p18,0.056,0.090), // yaw |
rkk | 20:6ae16da1492a | 5 | ch2(p17,0.054,0.092), // pitch |
rkk | 20:6ae16da1492a | 6 | ch3(p15,0.054,0.092), // amplitude |
rkk | 20:6ae16da1492a | 7 | ch4(p30,0.055,0.092), // adj |
rkk | 20:6ae16da1492a | 8 | ch6(p16,0.053,0.092), // frequency |
rkk | 17:6970aef8154b | 9 | mcon(p22, p6, p7), // change pin p5 to p7, because p5 is burned through |
rkk | 20:6ae16da1492a | 10 | //ap(p25, p26)//, |
rkk | 20:6ae16da1492a | 11 | leftservo(p21), |
rkk | 20:6ae16da1492a | 12 | rightservo(p23) |
rkk | 17:6970aef8154b | 13 | |
rkk | 11:8ec915eb70f6 | 14 | { |
rkk | 12:7eeb29892625 | 15 | wait_ms(50); |
rkk | 19:655db88b045c | 16 | amp = 0.0; |
rkk | 17:6970aef8154b | 17 | frq = 1.0; |
rkk | 19:655db88b045c | 18 | frqCmd = frq; |
rkk | 19:655db88b045c | 19 | yaw = 0.5; |
rkk | 17:6970aef8154b | 20 | pitch = 0.5; |
rkk | 20:6ae16da1492a | 21 | adj = 0.5; |
rkk | 20:6ae16da1492a | 22 | frqMin = 0.8; //hardcoded |
rkk | 20:6ae16da1492a | 23 | frqMax = 2.9; //hardcoded |
rkk | 19:655db88b045c | 24 | //change = 0; |
rkk | 19:655db88b045c | 25 | //state = 0; |
rkk | 19:655db88b045c | 26 | fullCycle = true; |
rkk | 19:655db88b045c | 27 | volume = 0.0; |
rkk | 19:655db88b045c | 28 | volMax = 0.1; |
rkk | 19:655db88b045c | 29 | volChg = 0.0; |
rkk | 20:6ae16da1492a | 30 | raiser = 0.0; |
rkk | 20:6ae16da1492a | 31 | pitAvg = 0.0; |
rkk | 20:6ae16da1492a | 32 | alPi = 0.2/(0.2+0.001);//tf/(tf+Ts); |
rkk | 20:6ae16da1492a | 33 | |
rkk | 20:6ae16da1492a | 34 | //leftservo.calibrate(0.0008, 45); |
rkk | 20:6ae16da1492a | 35 | //rightservo.calibrate(-0.0001, 45); |
rkk | 20:6ae16da1492a | 36 | |
rkk | 11:8ec915eb70f6 | 37 | } |
rkk | 11:8ec915eb70f6 | 38 | |
rkk | 11:8ec915eb70f6 | 39 | void MainController::control() |
rkk | 11:8ec915eb70f6 | 40 | { |
rkk | 11:8ec915eb70f6 | 41 | curTime = timer1.read(); |
rkk | 15:dc5753a5b83e | 42 | |
rkk | 19:655db88b045c | 43 | // check every half cycle |
rkk | 19:655db88b045c | 44 | if(curTime > 1/(2*frqCmd) ) { |
rkk | 19:655db88b045c | 45 | |
rkk | 19:655db88b045c | 46 | // read new yaw value every half cycle |
rkk | 19:655db88b045c | 47 | yaw = this->calculateYaw(); // a value from -1 to 1 |
rkk | 19:655db88b045c | 48 | |
rkk | 20:6ae16da1492a | 49 | if(yaw < 0.1 && yaw > -0.1){ |
rkk | 19:655db88b045c | 50 | yaw =0.0; |
rkk | 19:655db88b045c | 51 | } |
rkk | 19:655db88b045c | 52 | // calculate liquid volume change in the chambers |
rkk | 19:655db88b045c | 53 | volChg = volMax * yaw; |
rkk | 20:6ae16da1492a | 54 | volChg = 0.0; |
rkk | 19:655db88b045c | 55 | |
rkk | 19:655db88b045c | 56 | timeAdd = 0.0; |
rkk | 19:655db88b045c | 57 | |
rkk | 19:655db88b045c | 58 | // Read volume and frequency only every full cycle |
rkk | 19:655db88b045c | 59 | if( fullCycle ) { |
rkk | 19:655db88b045c | 60 | //read other new inputs |
rkk | 19:655db88b045c | 61 | amp = this->calculateAmplitude(); // a value from 0 to 1 |
rkk | 19:655db88b045c | 62 | frq = this->calculateFrequency(); // a value from frqmin to frqmax |
rkk | 19:655db88b045c | 63 | |
rkk | 19:655db88b045c | 64 | if(volChg > 0.0) { |
rkk | 19:655db88b045c | 65 | // adjust frequency to add additional volume |
rkk | 19:655db88b045c | 66 | if( amp < 0.0001 ) { |
rkk | 19:655db88b045c | 67 | amp = 0.0001; |
rkk | 19:655db88b045c | 68 | } |
rkk | 19:655db88b045c | 69 | timeAdd = volChg/amp; |
rkk | 19:655db88b045c | 70 | |
rkk | 19:655db88b045c | 71 | if( timeAdd > 0.5/frq ) { |
rkk | 19:655db88b045c | 72 | timeAdd = 0.5/frq; |
rkk | 19:655db88b045c | 73 | volChg = timeAdd * amp; |
rkk | 19:655db88b045c | 74 | } |
rkk | 19:655db88b045c | 75 | } |
rkk | 20:6ae16da1492a | 76 | ampNew = amp; |
rkk | 19:655db88b045c | 77 | |
rkk | 20:6ae16da1492a | 78 | if(yaw < 0.0) |
rkk | 20:6ae16da1492a | 79 | { |
rkk | 20:6ae16da1492a | 80 | ampNew = (1.0+0.7*yaw)*amp; |
rkk | 20:6ae16da1492a | 81 | } |
rkk | 19:655db88b045c | 82 | |
rkk | 19:655db88b045c | 83 | fullCycle = false; |
rkk | 19:655db88b045c | 84 | |
rkk | 19:655db88b045c | 85 | } else { |
rkk | 19:655db88b045c | 86 | // reverse for the downward slope |
rkk | 19:655db88b045c | 87 | amp = -amp; |
rkk | 19:655db88b045c | 88 | |
rkk | 19:655db88b045c | 89 | if(volChg < 0.0) { |
rkk | 19:655db88b045c | 90 | // adjust frequency to add additional volume |
rkk | 19:655db88b045c | 91 | if( amp > -0.0001 ) { |
rkk | 19:655db88b045c | 92 | amp = -0.0001; |
rkk | 19:655db88b045c | 93 | } |
rkk | 19:655db88b045c | 94 | timeAdd = volChg/amp; |
rkk | 19:655db88b045c | 95 | |
rkk | 19:655db88b045c | 96 | if( timeAdd > 0.5/frq ) { |
rkk | 19:655db88b045c | 97 | timeAdd = 0.5/frq; |
rkk | 19:655db88b045c | 98 | volChg = timeAdd * amp; |
rkk | 19:655db88b045c | 99 | } |
rkk | 19:655db88b045c | 100 | } |
rkk | 20:6ae16da1492a | 101 | |
rkk | 20:6ae16da1492a | 102 | ampNew = amp; |
rkk | 20:6ae16da1492a | 103 | |
rkk | 20:6ae16da1492a | 104 | if(yaw > 0.0) |
rkk | 20:6ae16da1492a | 105 | { |
rkk | 20:6ae16da1492a | 106 | ampNew = (1.0-0.7*yaw)*amp; |
rkk | 20:6ae16da1492a | 107 | } |
rkk | 19:655db88b045c | 108 | |
rkk | 19:655db88b045c | 109 | // use amp and frq from last cycle in order to make sure it is equalized |
rkk | 19:655db88b045c | 110 | fullCycle = true; |
rkk | 19:655db88b045c | 111 | } |
rkk | 19:655db88b045c | 112 | // update the frequency that actually needs to be commanded |
rkk | 19:655db88b045c | 113 | frqCmd = 1.0/( 2.0*( timeAdd + 1/( 2* frq) ) ); |
rkk | 19:655db88b045c | 114 | |
rkk | 20:6ae16da1492a | 115 | // read new yaw value every half cycle |
rkk | 20:6ae16da1492a | 116 | adj = this->calculateAdj(); // a value from 0 to 1 |
rkk | 20:6ae16da1492a | 117 | |
rkk | 19:655db88b045c | 118 | // for keeping track, calculate current volume storage, positive means on side is fuller, negative means other side is fuller |
rkk | 19:655db88b045c | 119 | //volume = volChg + volume; |
rkk | 19:655db88b045c | 120 | // rudder value used to define the trapezoid shape |
rkk | 20:6ae16da1492a | 121 | raiser = ( 5 * adj + 1.0); // varied from 1 to 5 |
rkk | 19:655db88b045c | 122 | |
rkk | 19:655db88b045c | 123 | //reset timer |
rkk | 11:8ec915eb70f6 | 124 | timer1.reset(); |
rkk | 11:8ec915eb70f6 | 125 | curTime = 0.0; |
rkk | 19:655db88b045c | 126 | } |
rkk | 15:dc5753a5b83e | 127 | |
rkk | 19:655db88b045c | 128 | |
rkk | 17:6970aef8154b | 129 | //Set Servo Values |
rkk | 20:6ae16da1492a | 130 | pitch = this->calculatePitch(); |
rkk | 20:6ae16da1492a | 131 | leftservo = pitch+0.03; |
rkk | 20:6ae16da1492a | 132 | rightservo = 1.0 - pitch; |
rkk | 20:6ae16da1492a | 133 | if (rightservo<0.03){ |
rkk | 20:6ae16da1492a | 134 | rightservo = 0.03; |
rkk | 20:6ae16da1492a | 135 | } |
rkk | 20:6ae16da1492a | 136 | |
rkk | 20:6ae16da1492a | 137 | dutyCycle = ampNew * saturate(raiser * sin( 2.0 * MATH_PI * frqCmd * curTime )); // add factor 4.0 to get a cut off sinus |
rkk | 19:655db88b045c | 138 | |
rkk | 11:8ec915eb70f6 | 139 | mcon.setpolarspeed(dutyCycle); |
rkk | 11:8ec915eb70f6 | 140 | } |
rkk | 12:7eeb29892625 | 141 | |
rkk | 11:8ec915eb70f6 | 142 | float MainController::calculateFrequency() |
rkk | 11:8ec915eb70f6 | 143 | { |
rkk | 20:6ae16da1492a | 144 | return ((frqMax - frqMin) * ch6.dutycyclescaledup() + frqMin); |
rkk | 11:8ec915eb70f6 | 145 | } |
rkk | 11:8ec915eb70f6 | 146 | |
rkk | 19:655db88b045c | 147 | float MainController::calculateAmplitude() |
rkk | 11:8ec915eb70f6 | 148 | { |
rkk | 20:6ae16da1492a | 149 | return (ch3.dutycyclescaledup()); |
rkk | 11:8ec915eb70f6 | 150 | } |
rkk | 11:8ec915eb70f6 | 151 | |
rkk | 19:655db88b045c | 152 | float MainController::calculateYaw() |
rkk | 12:7eeb29892625 | 153 | { |
rkk | 20:6ae16da1492a | 154 | return (2.0*ch1.dutycyclescaledup() - 1.0); |
rkk | 12:7eeb29892625 | 155 | } |
rkk | 12:7eeb29892625 | 156 | |
rkk | 17:6970aef8154b | 157 | float MainController::calculatePitch() |
rkk | 17:6970aef8154b | 158 | { |
rkk | 20:6ae16da1492a | 159 | pitAvg = alPi * pitAvg+ (1.0 - alPi)*(ch2.dutycyclescaledup()); |
rkk | 20:6ae16da1492a | 160 | return pitAvg; |
rkk | 20:6ae16da1492a | 161 | } |
rkk | 20:6ae16da1492a | 162 | |
rkk | 20:6ae16da1492a | 163 | float MainController::calculateAdj() |
rkk | 20:6ae16da1492a | 164 | { |
rkk | 20:6ae16da1492a | 165 | return (ch4.dutycyclescaledup()); |
rkk | 17:6970aef8154b | 166 | } |
rkk | 17:6970aef8154b | 167 | |
rkk | 11:8ec915eb70f6 | 168 | void MainController::start() |
rkk | 11:8ec915eb70f6 | 169 | { |
rkk | 11:8ec915eb70f6 | 170 | timer1.start(); |
rkk | 17:6970aef8154b | 171 | |
rkk | 12:7eeb29892625 | 172 | ticker1.attach(this, &MainController::control,0.001); |
rkk | 12:7eeb29892625 | 173 | //Autopilot guardian |
rkk | 12:7eeb29892625 | 174 | //ap.calibrate(); |
rkk | 12:7eeb29892625 | 175 | //ap.set2D(); |
rkk | 20:6ae16da1492a | 176 | //ap.setoff(); |
rkk | 12:7eeb29892625 | 177 | |
rkk | 11:8ec915eb70f6 | 178 | } |
rkk | 11:8ec915eb70f6 | 179 | |
rkk | 11:8ec915eb70f6 | 180 | void MainController::stop() |
rkk | 11:8ec915eb70f6 | 181 | { |
rkk | 11:8ec915eb70f6 | 182 | timer1.stop(); |
rkk | 11:8ec915eb70f6 | 183 | ticker1.detach(); |
rkk | 11:8ec915eb70f6 | 184 | mcon.setpolarspeed(0.0); |
rkk | 11:8ec915eb70f6 | 185 | } |
rkk | 11:8ec915eb70f6 | 186 | |
rkk | 11:8ec915eb70f6 | 187 | float MainController::getDutyCycle() |
rkk | 11:8ec915eb70f6 | 188 | { |
rkk | 11:8ec915eb70f6 | 189 | return dutyCycle; |
rkk | 11:8ec915eb70f6 | 190 | } |
rkk | 11:8ec915eb70f6 | 191 | |
rkk | 11:8ec915eb70f6 | 192 | float MainController::getAmplitude() |
rkk | 11:8ec915eb70f6 | 193 | { |
rkk | 19:655db88b045c | 194 | return amp; |
rkk | 11:8ec915eb70f6 | 195 | } |
rkk | 11:8ec915eb70f6 | 196 | |
rkk | 11:8ec915eb70f6 | 197 | |
rkk | 11:8ec915eb70f6 | 198 | float MainController::getFrequency() |
rkk | 11:8ec915eb70f6 | 199 | { |
rkk | 11:8ec915eb70f6 | 200 | return frq; |
rkk | 11:8ec915eb70f6 | 201 | } |
rkk | 11:8ec915eb70f6 | 202 | |
rkk | 11:8ec915eb70f6 | 203 | float MainController::getVolume() |
rkk | 11:8ec915eb70f6 | 204 | { |
rkk | 19:655db88b045c | 205 | return volume; |
rkk | 11:8ec915eb70f6 | 206 | } |
rkk | 12:7eeb29892625 | 207 | |
rkk | 19:655db88b045c | 208 | float MainController::getYaw() |
rkk | 12:7eeb29892625 | 209 | { |
rkk | 19:655db88b045c | 210 | return yaw; |
rkk | 12:7eeb29892625 | 211 | } |
rkk | 12:7eeb29892625 | 212 | |
rkk | 17:6970aef8154b | 213 | float MainController::getPitch() |
rkk | 17:6970aef8154b | 214 | { |
rkk | 17:6970aef8154b | 215 | return pitch; |
rkk | 17:6970aef8154b | 216 | } |
rkk | 17:6970aef8154b | 217 | |
rkk | 20:6ae16da1492a | 218 | float MainController::getAdj() |
rkk | 20:6ae16da1492a | 219 | { |
rkk | 20:6ae16da1492a | 220 | return adj; |
rkk | 20:6ae16da1492a | 221 | } |
rkk | 20:6ae16da1492a | 222 | |
rkk | 19:655db88b045c | 223 | float MainController::getTimeAdd() |
rkk | 19:655db88b045c | 224 | { |
rkk | 19:655db88b045c | 225 | return timeAdd; |
rkk | 19:655db88b045c | 226 | } |
rkk | 19:655db88b045c | 227 | |
rkk | 15:dc5753a5b83e | 228 | //signum function |
rkk | 13:5ed8fd870723 | 229 | float MainController::signum(float input) |
rkk | 13:5ed8fd870723 | 230 | { |
rkk | 13:5ed8fd870723 | 231 | if (input>0.0) |
rkk | 14:a5389e26a63b | 232 | return 1.0; |
rkk | 13:5ed8fd870723 | 233 | else if (input<0.0) |
rkk | 14:a5389e26a63b | 234 | return (-1.0); |
rkk | 13:5ed8fd870723 | 235 | else |
rkk | 14:a5389e26a63b | 236 | return 0.0; |
rkk | 15:dc5753a5b83e | 237 | } |
rkk | 15:dc5753a5b83e | 238 | |
rkk | 15:dc5753a5b83e | 239 | //saturate function |
rkk | 15:dc5753a5b83e | 240 | float MainController::saturate(float input) |
rkk | 15:dc5753a5b83e | 241 | { |
rkk | 15:dc5753a5b83e | 242 | if (input > 1.0) |
rkk | 15:dc5753a5b83e | 243 | return (1.0); |
rkk | 15:dc5753a5b83e | 244 | else if (input < -1.0) |
rkk | 15:dc5753a5b83e | 245 | return (-1.0); |
rkk | 15:dc5753a5b83e | 246 | else |
rkk | 15:dc5753a5b83e | 247 | return input; |
rkk | 20:6ae16da1492a | 248 | } |