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