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@22:807d5467fbf6, 2014-06-05 (annotated)
- Committer:
- sandwich
- Date:
- Thu Jun 05 00:13:02 2014 +0000
- Revision:
- 22:807d5467fbf6
- Parent:
- 20:6ae16da1492a
- Child:
- 23:ef1be367726e
look at the previous one
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() |
sandwich | 22:807d5467fbf6 | 4 | :ch1(p18,0.047,0.081), // yaw |
sandwich | 22:807d5467fbf6 | 5 | ch2(p17,0.047,0.081), // pitch |
sandwich | 22:807d5467fbf6 | 6 | ch3(p15,0.047,0.081), // amplitude |
sandwich | 22:807d5467fbf6 | 7 | ch4(p30,0.047,0.081), // adj |
sandwich | 22:807d5467fbf6 | 8 | ch6(p16,0.047,0.081), // frequency |
sandwich | 22:807d5467fbf6 | 9 | ch5(p29,0.047,0.081), // manual vs auto control |
sandwich | 22:807d5467fbf6 | 10 | syren(p9,p10), //syren is 38400bps serial |
rkk | 20:6ae16da1492a | 11 | //ap(p25, p26)//, |
rkk | 20:6ae16da1492a | 12 | leftservo(p21), |
sandwich | 22:807d5467fbf6 | 13 | rightservo(p23), |
sandwich | 22:807d5467fbf6 | 14 | pcam(p11,p12,p13,10), |
sandwich | 22:807d5467fbf6 | 15 | controlThread(&MainController::controlThreadStarter, this, osPriorityRealtime, 1024), |
sandwich | 22:807d5467fbf6 | 16 | trackerThread(&MainController::trackerThreadStarter, this, osPriorityHigh, 1024) |
rkk | 11:8ec915eb70f6 | 17 | { |
rkk | 12:7eeb29892625 | 18 | wait_ms(50); |
rkk | 19:655db88b045c | 19 | amp = 0.0; |
rkk | 17:6970aef8154b | 20 | frq = 1.0; |
rkk | 19:655db88b045c | 21 | frqCmd = frq; |
rkk | 19:655db88b045c | 22 | yaw = 0.5; |
rkk | 17:6970aef8154b | 23 | pitch = 0.5; |
rkk | 20:6ae16da1492a | 24 | adj = 0.5; |
rkk | 20:6ae16da1492a | 25 | frqMin = 0.8; //hardcoded |
rkk | 20:6ae16da1492a | 26 | frqMax = 2.9; //hardcoded |
rkk | 19:655db88b045c | 27 | //change = 0; |
rkk | 19:655db88b045c | 28 | //state = 0; |
rkk | 19:655db88b045c | 29 | fullCycle = true; |
rkk | 19:655db88b045c | 30 | volume = 0.0; |
rkk | 19:655db88b045c | 31 | volMax = 0.1; |
rkk | 19:655db88b045c | 32 | volChg = 0.0; |
rkk | 20:6ae16da1492a | 33 | raiser = 0.0; |
rkk | 20:6ae16da1492a | 34 | pitAvg = 0.0; |
rkk | 20:6ae16da1492a | 35 | alPi = 0.2/(0.2+0.001);//tf/(tf+Ts); |
sandwich | 22:807d5467fbf6 | 36 | syren.baud(38400); |
sandwich | 22:807d5467fbf6 | 37 | //pcam=new pixySPI(p11,p12,p13,10); |
sandwich | 22:807d5467fbf6 | 38 | |
sandwich | 22:807d5467fbf6 | 39 | //leftservo.calibrate(0.0008, 45); |
sandwich | 22:807d5467fbf6 | 40 | //rightservo.calibrate(-0.0001, 45); |
sandwich | 22:807d5467fbf6 | 41 | |
sandwich | 22:807d5467fbf6 | 42 | } |
rkk | 20:6ae16da1492a | 43 | |
sandwich | 22:807d5467fbf6 | 44 | MainController::~MainController() |
sandwich | 22:807d5467fbf6 | 45 | { |
sandwich | 22:807d5467fbf6 | 46 | //delete pcam; |
sandwich | 22:807d5467fbf6 | 47 | } |
sandwich | 22:807d5467fbf6 | 48 | |
sandwich | 22:807d5467fbf6 | 49 | void MainController::controlThreadStarter(void const *p) |
sandwich | 22:807d5467fbf6 | 50 | { |
sandwich | 22:807d5467fbf6 | 51 | MainController *instance = (MainController*)p; |
sandwich | 22:807d5467fbf6 | 52 | instance->control(); |
sandwich | 22:807d5467fbf6 | 53 | } |
sandwich | 22:807d5467fbf6 | 54 | |
sandwich | 22:807d5467fbf6 | 55 | void MainController::trackerThreadStarter(void const *p) |
sandwich | 22:807d5467fbf6 | 56 | { |
sandwich | 22:807d5467fbf6 | 57 | MainController *instance = (MainController*)p; |
sandwich | 22:807d5467fbf6 | 58 | instance->trackTarget(); |
rkk | 11:8ec915eb70f6 | 59 | } |
rkk | 11:8ec915eb70f6 | 60 | |
rkk | 11:8ec915eb70f6 | 61 | void MainController::control() |
rkk | 11:8ec915eb70f6 | 62 | { |
sandwich | 22:807d5467fbf6 | 63 | controlThread.signal_wait(START_THREAD); |
sandwich | 22:807d5467fbf6 | 64 | while (1) { |
sandwich | 22:807d5467fbf6 | 65 | trackMutex.lock(); |
sandwich | 22:807d5467fbf6 | 66 | curTime = timer1.read(); |
sandwich | 22:807d5467fbf6 | 67 | //Block bestObject=pcam.getBlocks()[0]; //our best bet is the first block because it has the biggest area |
sandwich | 22:807d5467fbf6 | 68 | |
sandwich | 22:807d5467fbf6 | 69 | // check every half cycle |
sandwich | 22:807d5467fbf6 | 70 | if(curTime > 1/(2*frqCmd) ) { |
sandwich | 22:807d5467fbf6 | 71 | // read new yaw value every half cycle |
sandwich | 22:807d5467fbf6 | 72 | if (ch5.dutycyclescaledup()<=0.5) |
sandwich | 22:807d5467fbf6 | 73 | yaw = this->calculateYaw(); // a value from -1 to 1 |
sandwich | 22:807d5467fbf6 | 74 | else { |
sandwich | 22:807d5467fbf6 | 75 | //do proportional control on fish yaw |
sandwich | 22:807d5467fbf6 | 76 | float gain=1/float(CENTER_X); |
sandwich | 22:807d5467fbf6 | 77 | float error=pcam.getBestX()-CENTER_X; //calculate yaw difference |
sandwich | 22:807d5467fbf6 | 78 | yaw=error*gain; |
sandwich | 22:807d5467fbf6 | 79 | //printf("xlocation: %d -> %f\n", pcam.getBestX(), yaw); |
sandwich | 22:807d5467fbf6 | 80 | } |
sandwich | 22:807d5467fbf6 | 81 | |
sandwich | 22:807d5467fbf6 | 82 | if(yaw < 0.1 && yaw > -0.1) { |
sandwich | 22:807d5467fbf6 | 83 | yaw =0.0; |
sandwich | 22:807d5467fbf6 | 84 | } |
sandwich | 22:807d5467fbf6 | 85 | // calculate liquid volume change in the chambers |
sandwich | 22:807d5467fbf6 | 86 | volChg = volMax * yaw; |
sandwich | 22:807d5467fbf6 | 87 | volChg = 0.0; |
sandwich | 22:807d5467fbf6 | 88 | |
sandwich | 22:807d5467fbf6 | 89 | timeAdd = 0.0; |
rkk | 19:655db88b045c | 90 | |
sandwich | 22:807d5467fbf6 | 91 | // Read volume and frequency only every full cycle |
sandwich | 22:807d5467fbf6 | 92 | if( fullCycle ) { |
sandwich | 22:807d5467fbf6 | 93 | //read other new inputs |
sandwich | 22:807d5467fbf6 | 94 | amp = this->calculateAmplitude(); // a value from 0 to 1 |
sandwich | 22:807d5467fbf6 | 95 | frq = this->calculateFrequency(); // a value from frqmin to frqmax |
rkk | 19:655db88b045c | 96 | |
sandwich | 22:807d5467fbf6 | 97 | if(volChg > 0.0) { |
sandwich | 22:807d5467fbf6 | 98 | // adjust frequency to add additional volume |
sandwich | 22:807d5467fbf6 | 99 | if( amp < 0.0001 ) { |
sandwich | 22:807d5467fbf6 | 100 | amp = 0.0001; |
sandwich | 22:807d5467fbf6 | 101 | } |
sandwich | 22:807d5467fbf6 | 102 | timeAdd = volChg/amp; |
sandwich | 22:807d5467fbf6 | 103 | |
sandwich | 22:807d5467fbf6 | 104 | if( timeAdd > 0.5/frq ) { |
sandwich | 22:807d5467fbf6 | 105 | timeAdd = 0.5/frq; |
sandwich | 22:807d5467fbf6 | 106 | volChg = timeAdd * amp; |
sandwich | 22:807d5467fbf6 | 107 | } |
rkk | 19:655db88b045c | 108 | } |
sandwich | 22:807d5467fbf6 | 109 | ampNew = amp; |
sandwich | 22:807d5467fbf6 | 110 | |
sandwich | 22:807d5467fbf6 | 111 | if(yaw < 0.0) { |
sandwich | 22:807d5467fbf6 | 112 | ampNew = (1.0+0.7*yaw)*amp; |
sandwich | 22:807d5467fbf6 | 113 | } |
sandwich | 22:807d5467fbf6 | 114 | |
sandwich | 22:807d5467fbf6 | 115 | fullCycle = false; |
rkk | 19:655db88b045c | 116 | |
sandwich | 22:807d5467fbf6 | 117 | } else { |
sandwich | 22:807d5467fbf6 | 118 | // reverse for the downward slope |
sandwich | 22:807d5467fbf6 | 119 | amp = -amp; |
sandwich | 22:807d5467fbf6 | 120 | |
sandwich | 22:807d5467fbf6 | 121 | if(volChg < 0.0) { |
sandwich | 22:807d5467fbf6 | 122 | // adjust frequency to add additional volume |
sandwich | 22:807d5467fbf6 | 123 | if( amp > -0.0001 ) { |
sandwich | 22:807d5467fbf6 | 124 | amp = -0.0001; |
sandwich | 22:807d5467fbf6 | 125 | } |
sandwich | 22:807d5467fbf6 | 126 | timeAdd = volChg/amp; |
rkk | 19:655db88b045c | 127 | |
sandwich | 22:807d5467fbf6 | 128 | if( timeAdd > 0.5/frq ) { |
sandwich | 22:807d5467fbf6 | 129 | timeAdd = 0.5/frq; |
sandwich | 22:807d5467fbf6 | 130 | volChg = timeAdd * amp; |
sandwich | 22:807d5467fbf6 | 131 | } |
rkk | 19:655db88b045c | 132 | } |
sandwich | 22:807d5467fbf6 | 133 | |
sandwich | 22:807d5467fbf6 | 134 | ampNew = amp; |
sandwich | 22:807d5467fbf6 | 135 | |
sandwich | 22:807d5467fbf6 | 136 | if(yaw > 0.0) { |
sandwich | 22:807d5467fbf6 | 137 | ampNew = (1.0-0.7*yaw)*amp; |
rkk | 19:655db88b045c | 138 | } |
sandwich | 22:807d5467fbf6 | 139 | |
sandwich | 22:807d5467fbf6 | 140 | // use amp and frq from last cycle in order to make sure it is equalized |
sandwich | 22:807d5467fbf6 | 141 | fullCycle = true; |
rkk | 19:655db88b045c | 142 | } |
sandwich | 22:807d5467fbf6 | 143 | // update the frequency that actually needs to be commanded |
sandwich | 22:807d5467fbf6 | 144 | frqCmd = 1.0/( 2.0*( timeAdd + 1/( 2* frq) ) ); |
sandwich | 22:807d5467fbf6 | 145 | |
sandwich | 22:807d5467fbf6 | 146 | // read new yaw value every half cycle |
sandwich | 22:807d5467fbf6 | 147 | adj = this->calculateAdj(); // a value from 0 to 1 |
sandwich | 22:807d5467fbf6 | 148 | |
sandwich | 22:807d5467fbf6 | 149 | // for keeping track, calculate current volume storage, positive means on side is fuller, negative means other side is fuller |
sandwich | 22:807d5467fbf6 | 150 | //volume = volChg + volume; |
sandwich | 22:807d5467fbf6 | 151 | // rudder value used to define the trapezoid shape |
sandwich | 22:807d5467fbf6 | 152 | raiser = ( 5 * adj + 1.0); // varied from 1 to 5 |
sandwich | 22:807d5467fbf6 | 153 | |
sandwich | 22:807d5467fbf6 | 154 | //reset timer |
sandwich | 22:807d5467fbf6 | 155 | timer1.reset(); |
sandwich | 22:807d5467fbf6 | 156 | curTime = 0.0; |
rkk | 19:655db88b045c | 157 | } |
sandwich | 22:807d5467fbf6 | 158 | |
rkk | 19:655db88b045c | 159 | |
sandwich | 22:807d5467fbf6 | 160 | //Set Servo Values |
sandwich | 22:807d5467fbf6 | 161 | if (ch5.dutycyclescaledup()<=0.5) |
sandwich | 22:807d5467fbf6 | 162 | pitch = this->calculatePitch(); |
sandwich | 22:807d5467fbf6 | 163 | else { |
sandwich | 22:807d5467fbf6 | 164 | //do a proportional control on the error since servos already give you position control |
sandwich | 22:807d5467fbf6 | 165 | float gain=1/float(CENTER_Y); //try this out for now |
sandwich | 22:807d5467fbf6 | 166 | float error=pcam.getBestY()-CENTER_Y; |
sandwich | 22:807d5467fbf6 | 167 | pitch=error*gain; |
sandwich | 22:807d5467fbf6 | 168 | //printf("ylocation: %d -> %f\n", pcam.getBestY(), pitch); |
sandwich | 22:807d5467fbf6 | 169 | } |
sandwich | 22:807d5467fbf6 | 170 | leftservo = pitch+0.03; |
sandwich | 22:807d5467fbf6 | 171 | rightservo = 1.0 - pitch; |
sandwich | 22:807d5467fbf6 | 172 | if (rightservo<0.03) { |
sandwich | 22:807d5467fbf6 | 173 | rightservo = 0.03; |
sandwich | 22:807d5467fbf6 | 174 | } |
sandwich | 22:807d5467fbf6 | 175 | |
sandwich | 22:807d5467fbf6 | 176 | dutyCycle = ampNew * saturate(raiser * sin( 2.0 * MATH_PI * frqCmd * curTime )); // add factor 4.0 to get a cut off sinus |
sandwich | 22:807d5467fbf6 | 177 | |
sandwich | 22:807d5467fbf6 | 178 | //now scale duty cycle from -1 to 1 -> 0 to 255 |
sandwich | 22:807d5467fbf6 | 179 | dutyCycle=(255/2)*dutyCycle+127; |
sandwich | 22:807d5467fbf6 | 180 | if (dutyCycle<0) |
sandwich | 22:807d5467fbf6 | 181 | dutyCycle=0.0; |
sandwich | 22:807d5467fbf6 | 182 | else if (dutyCycle>255) |
sandwich | 22:807d5467fbf6 | 183 | dutyCycle=255.0; |
sandwich | 22:807d5467fbf6 | 184 | |
sandwich | 22:807d5467fbf6 | 185 | //mcon.setpolarspeed(dutyCycle); |
sandwich | 22:807d5467fbf6 | 186 | syren.putc(int(dutyCycle)); |
sandwich | 22:807d5467fbf6 | 187 | //syren.putc(int(0)); |
sandwich | 22:807d5467fbf6 | 188 | trackMutex.unlock(); |
sandwich | 22:807d5467fbf6 | 189 | Thread::wait(1); |
rkk | 19:655db88b045c | 190 | } |
sandwich | 22:807d5467fbf6 | 191 | } |
sandwich | 22:807d5467fbf6 | 192 | |
sandwich | 22:807d5467fbf6 | 193 | void MainController::trackTarget() |
sandwich | 22:807d5467fbf6 | 194 | { |
sandwich | 22:807d5467fbf6 | 195 | trackerThread.signal_wait(START_THREAD); |
sandwich | 22:807d5467fbf6 | 196 | while (1) { |
sandwich | 22:807d5467fbf6 | 197 | //trackMutex.lock(); |
sandwich | 22:807d5467fbf6 | 198 | //printf("tracking\n"); |
sandwich | 22:807d5467fbf6 | 199 | pcam.capture(); |
sandwich | 22:807d5467fbf6 | 200 | //trackMutex.unlock(); |
rkk | 20:6ae16da1492a | 201 | } |
rkk | 11:8ec915eb70f6 | 202 | } |
rkk | 12:7eeb29892625 | 203 | |
rkk | 11:8ec915eb70f6 | 204 | float MainController::calculateFrequency() |
rkk | 11:8ec915eb70f6 | 205 | { |
sandwich | 22:807d5467fbf6 | 206 | return ((frqMax - frqMin) * ch6.dutycyclescaledup() + frqMin); |
rkk | 11:8ec915eb70f6 | 207 | } |
rkk | 11:8ec915eb70f6 | 208 | |
rkk | 19:655db88b045c | 209 | float MainController::calculateAmplitude() |
rkk | 11:8ec915eb70f6 | 210 | { |
rkk | 20:6ae16da1492a | 211 | return (ch3.dutycyclescaledup()); |
rkk | 11:8ec915eb70f6 | 212 | } |
rkk | 11:8ec915eb70f6 | 213 | |
rkk | 19:655db88b045c | 214 | float MainController::calculateYaw() |
rkk | 12:7eeb29892625 | 215 | { |
sandwich | 22:807d5467fbf6 | 216 | return (2.0*ch1.dutycyclescaledup() - 1.0); |
rkk | 12:7eeb29892625 | 217 | } |
rkk | 12:7eeb29892625 | 218 | |
rkk | 17:6970aef8154b | 219 | float MainController::calculatePitch() |
rkk | 17:6970aef8154b | 220 | { |
rkk | 20:6ae16da1492a | 221 | pitAvg = alPi * pitAvg+ (1.0 - alPi)*(ch2.dutycyclescaledup()); |
rkk | 20:6ae16da1492a | 222 | return pitAvg; |
rkk | 20:6ae16da1492a | 223 | } |
rkk | 20:6ae16da1492a | 224 | |
rkk | 20:6ae16da1492a | 225 | float MainController::calculateAdj() |
rkk | 20:6ae16da1492a | 226 | { |
rkk | 20:6ae16da1492a | 227 | return (ch4.dutycyclescaledup()); |
rkk | 17:6970aef8154b | 228 | } |
rkk | 17:6970aef8154b | 229 | |
rkk | 11:8ec915eb70f6 | 230 | void MainController::start() |
rkk | 11:8ec915eb70f6 | 231 | { |
rkk | 11:8ec915eb70f6 | 232 | timer1.start(); |
sandwich | 22:807d5467fbf6 | 233 | |
sandwich | 22:807d5467fbf6 | 234 | printf("start thread\n"); |
sandwich | 22:807d5467fbf6 | 235 | controlThread.signal_set(START_THREAD); |
sandwich | 22:807d5467fbf6 | 236 | trackerThread.signal_set(START_THREAD); |
sandwich | 22:807d5467fbf6 | 237 | //ticker1.attach(this, &MainController::control,0.001); |
sandwich | 22:807d5467fbf6 | 238 | //control=Thread(&MainController::control); |
sandwich | 22:807d5467fbf6 | 239 | //tracker.attach(this, &MainController::trackTarget, 2.00); //update target position every second |
rkk | 12:7eeb29892625 | 240 | //Autopilot guardian |
rkk | 12:7eeb29892625 | 241 | //ap.calibrate(); |
rkk | 12:7eeb29892625 | 242 | //ap.set2D(); |
rkk | 20:6ae16da1492a | 243 | //ap.setoff(); |
sandwich | 22:807d5467fbf6 | 244 | |
rkk | 11:8ec915eb70f6 | 245 | } |
rkk | 11:8ec915eb70f6 | 246 | |
rkk | 11:8ec915eb70f6 | 247 | void MainController::stop() |
rkk | 11:8ec915eb70f6 | 248 | { |
rkk | 11:8ec915eb70f6 | 249 | timer1.stop(); |
sandwich | 22:807d5467fbf6 | 250 | //ticker1.detach(); |
sandwich | 22:807d5467fbf6 | 251 | //tracker.detach(); |
sandwich | 22:807d5467fbf6 | 252 | //mcon.setpolarspeed(0.0); |
sandwich | 22:807d5467fbf6 | 253 | syren.putc(int(127)); |
rkk | 11:8ec915eb70f6 | 254 | } |
rkk | 11:8ec915eb70f6 | 255 | |
rkk | 11:8ec915eb70f6 | 256 | float MainController::getDutyCycle() |
rkk | 11:8ec915eb70f6 | 257 | { |
rkk | 11:8ec915eb70f6 | 258 | return dutyCycle; |
rkk | 11:8ec915eb70f6 | 259 | } |
rkk | 11:8ec915eb70f6 | 260 | |
rkk | 11:8ec915eb70f6 | 261 | float MainController::getAmplitude() |
rkk | 11:8ec915eb70f6 | 262 | { |
rkk | 19:655db88b045c | 263 | return amp; |
rkk | 11:8ec915eb70f6 | 264 | } |
rkk | 11:8ec915eb70f6 | 265 | |
rkk | 11:8ec915eb70f6 | 266 | |
rkk | 11:8ec915eb70f6 | 267 | float MainController::getFrequency() |
rkk | 11:8ec915eb70f6 | 268 | { |
rkk | 11:8ec915eb70f6 | 269 | return frq; |
rkk | 11:8ec915eb70f6 | 270 | } |
rkk | 11:8ec915eb70f6 | 271 | |
rkk | 11:8ec915eb70f6 | 272 | float MainController::getVolume() |
rkk | 11:8ec915eb70f6 | 273 | { |
rkk | 19:655db88b045c | 274 | return volume; |
rkk | 11:8ec915eb70f6 | 275 | } |
rkk | 12:7eeb29892625 | 276 | |
rkk | 19:655db88b045c | 277 | float MainController::getYaw() |
rkk | 12:7eeb29892625 | 278 | { |
rkk | 19:655db88b045c | 279 | return yaw; |
rkk | 12:7eeb29892625 | 280 | } |
rkk | 12:7eeb29892625 | 281 | |
rkk | 17:6970aef8154b | 282 | float MainController::getPitch() |
rkk | 17:6970aef8154b | 283 | { |
rkk | 17:6970aef8154b | 284 | return pitch; |
rkk | 17:6970aef8154b | 285 | } |
rkk | 17:6970aef8154b | 286 | |
rkk | 20:6ae16da1492a | 287 | float MainController::getAdj() |
rkk | 20:6ae16da1492a | 288 | { |
rkk | 20:6ae16da1492a | 289 | return adj; |
rkk | 20:6ae16da1492a | 290 | } |
rkk | 20:6ae16da1492a | 291 | |
rkk | 19:655db88b045c | 292 | float MainController::getTimeAdd() |
rkk | 19:655db88b045c | 293 | { |
rkk | 19:655db88b045c | 294 | return timeAdd; |
rkk | 19:655db88b045c | 295 | } |
rkk | 19:655db88b045c | 296 | |
sandwich | 22:807d5467fbf6 | 297 | bool MainController::getOpMode() |
sandwich | 22:807d5467fbf6 | 298 | { |
sandwich | 22:807d5467fbf6 | 299 | return ch5.dutycyclescaledup()>0.5; |
sandwich | 22:807d5467fbf6 | 300 | } |
sandwich | 22:807d5467fbf6 | 301 | |
rkk | 15:dc5753a5b83e | 302 | //signum function |
rkk | 13:5ed8fd870723 | 303 | float MainController::signum(float input) |
rkk | 13:5ed8fd870723 | 304 | { |
rkk | 13:5ed8fd870723 | 305 | if (input>0.0) |
rkk | 14:a5389e26a63b | 306 | return 1.0; |
rkk | 13:5ed8fd870723 | 307 | else if (input<0.0) |
rkk | 14:a5389e26a63b | 308 | return (-1.0); |
rkk | 13:5ed8fd870723 | 309 | else |
rkk | 14:a5389e26a63b | 310 | return 0.0; |
rkk | 15:dc5753a5b83e | 311 | } |
rkk | 15:dc5753a5b83e | 312 | |
rkk | 15:dc5753a5b83e | 313 | //saturate function |
rkk | 15:dc5753a5b83e | 314 | float MainController::saturate(float input) |
rkk | 15:dc5753a5b83e | 315 | { |
rkk | 15:dc5753a5b83e | 316 | if (input > 1.0) |
rkk | 15:dc5753a5b83e | 317 | return (1.0); |
rkk | 15:dc5753a5b83e | 318 | else if (input < -1.0) |
rkk | 15:dc5753a5b83e | 319 | return (-1.0); |
rkk | 15:dc5753a5b83e | 320 | else |
rkk | 15:dc5753a5b83e | 321 | return input; |
rkk | 20:6ae16da1492a | 322 | } |