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
Revision 19:655db88b045c, committed 2014-05-28
- Comitter:
- rkk
- Date:
- Wed May 28 01:48:23 2014 +0000
- Parent:
- 18:9ba4566f2361
- Child:
- 20:6ae16da1492a
- Commit message:
- added yaw controller with adjustable frequency for either half to do a turning motion
Changed in this revision
--- a/MainController.cpp Thu May 22 13:35:01 2014 +0000 +++ b/MainController.cpp Wed May 28 01:48:23 2014 +0000 @@ -13,14 +13,20 @@ { wait_ms(50); - vol = 0.0; + amp = 0.0; frq = 1.0; - rud = 0.5; + frqCmd = frq; + yaw = 0.5; pitch = 0.5; frqMin = 0.4; // hardcoded frqMax = 2.5; //hardcoded - change = 0; - state = 0; + //change = 0; + //state = 0; + fullCycle = true; + volume = 0.0; + volMax = 0.1; + volChg = 0.0; + ampCmd = 0.0; //goofftime = 0.0; //switched = false; } @@ -29,77 +35,97 @@ { curTime = timer1.read(); - if(curTime > 1/frq) { - //read new inputs - vol = this->calculateVolume(); - frq = this->calculateFrequency(); - rud = this->calculateRudder(); + // check every half cycle + if(curTime > 1/(2*frqCmd) ) { + + // read new yaw value every half cycle + yaw = this->calculateYaw(); // a value from -1 to 1 + + if(yaw < 0.075 && yaw > -0.075){ + yaw =0.0; + } + // calculate liquid volume change in the chambers + volChg = volMax * yaw; + //volChg = 0.0; + + timeAdd = 0.0; + + // Read volume and frequency only every full cycle + if( fullCycle ) { + //read other new inputs + amp = this->calculateAmplitude(); // a value from 0 to 1 + frq = this->calculateFrequency(); // a value from frqmin to frqmax + + if(volChg > 0.0) { + // adjust frequency to add additional volume + if( amp < 0.0001 ) { + amp = 0.0001; + } + timeAdd = volChg/amp; + + if( timeAdd > 0.5/frq ) { + timeAdd = 0.5/frq; + volChg = timeAdd * amp; + } + } + +// if(yaw >0.0) +// { +// ampNew = amp + yaw*amp; +// ampNew = (ampNew > 1.0) ? 1.0 : ampNew; +// +// } + + fullCycle = false; + + } else { + // reverse for the downward slope + amp = -amp; + + if(volChg < 0.0) { + // adjust frequency to add additional volume + if( amp > -0.0001 ) { + amp = -0.0001; + } + timeAdd = volChg/amp; + + if( timeAdd > 0.5/frq ) { + timeAdd = 0.5/frq; + volChg = timeAdd * amp; + } + } +// if(yaw < 0.0) +// { +// ampNew = amp - yaw*amp; +// ampNew = (ampNew < -1.0) ? -1.0 : ampNew; +// +// } + + // use amp and frq from last cycle in order to make sure it is equalized + fullCycle = true; + } + // update the frequency that actually needs to be commanded + frqCmd = 1.0/( 2.0*( timeAdd + 1/( 2* frq) ) ); + + // for keeping track, calculate current volume storage, positive means on side is fuller, negative means other side is fuller + //volume = volChg + volume; + // rudder value used to define the trapezoid shape + // ampCmd = vol * ( 2* rud + 1.0); // varied from 1 to 5 + ampCmd = amp * 2.0; // scale it up + + //reset timer timer1.reset(); curTime = 0.0; - - // rudder value used to define the trapezoid shape - // amplitude = vol * ( 2* rud + 1.0); // varied from 1 to 5 - amplitude = vol * 4.0; - - //reset change to zero - change = 0; + } - if(state == 1) { - if(rud < 0.75) { - change = -1; - state = 0; - } - } - else if (state == -1) { - if(rud> 0.25) { - change = 1; - state = 0; - } - } - else { - if(rud < 0.25) { - change = -1; - state = -1; - } else if(rud > 0.75) { - change = 1; - state = 1; - } - } - //switched = false; - //goofftime = (vol/(2*frq)); - } - + //Set Servo Values //pitch = this->calculatePitch(); //leftservo = pitch; //rightservo = 1.0 - pitch; - -// if (curTime > 1/(2*frq) && (switched == false)) -// { -// switched = true; -// amplitude = -1.0; -// } -// -// if(!switched) -// { -// if(curTime > goofftime ) -// amplitude = 0.0; -// } -// else -// { -// if(curTime > (1/(2*frq)+goofftime)) -// amplitude = 0.0; -// } - // saturate ensures the duty cycle does not exceed 1, - - if((change == 1) && (curTime > 1/(2*frq))) { - dutyCycle = 0.0; - } else if((change == -1) && (curTime < 1/(2*frq))) { - dutyCycle = 0.0; - } else { - dutyCycle = saturate(amplitude * sin( 2.0 * MATH_PI * frq * curTime )); - } + dutyCycle = saturate(ampCmd * sin( 2.0 * MATH_PI * frqCmd * curTime )); // add factor 4.0 to get a cut off sinus + mcon.setpolarspeed(dutyCycle); } @@ -108,14 +134,14 @@ return ((frqMax - frqMin) * ch3.dutycyclescaledup() + frqMin); } -float MainController::calculateVolume() +float MainController::calculateAmplitude() { return (ch6.dutycyclescaledup()); } -float MainController::calculateRudder() +float MainController::calculateYaw() { - return (ch4.dutycyclescaledup()); + return (2.0*ch4.dutycyclescaledup() - 1.0); } float MainController::calculatePitch() @@ -149,7 +175,7 @@ float MainController::getAmplitude() { - return amplitude; + return amp; } @@ -160,12 +186,12 @@ float MainController::getVolume() { - return vol; + return volume; } -float MainController::getRudder() +float MainController::getYaw() { - return rud; + return yaw; } float MainController::getPitch() @@ -173,6 +199,11 @@ return pitch; } +float MainController::getTimeAdd() +{ + return timeAdd; +} + //signum function float MainController::signum(float input) {
--- a/MainController.h Thu May 22 13:35:01 2014 +0000 +++ b/MainController.h Wed May 28 01:48:23 2014 +0000 @@ -34,8 +34,9 @@ float getFrequency(); float getVolume(); float getAmplitude(); - float getRudder(); + float getYaw(); float getPitch(); + float getTimeAdd(); /** Stop the main controller * @@ -47,8 +48,8 @@ protected: void control(); float calculateFrequency(); - float calculateVolume(); - float calculateRudder(); + float calculateAmplitude(); + float calculateYaw(); float calculatePitch(); float signum(float input); float saturate(float input); @@ -66,18 +67,25 @@ Timer timer1; Ticker ticker1; - float vol; + float amp; float frq; float dutyCycle; float curTime; float frqMin; float frqMax; - float amplitude; - float rud; + float yaw; float pitch; - int state; - int change; + bool fullCycle; + float volume; + float volChg; + float volMax; + float frqCmd; + float timeAdd; + float ampNew; + float ampCmd; + //int state; + //int change; //float goofftime; //bool switched; };
--- a/main.cpp Thu May 22 13:35:01 2014 +0000 +++ b/main.cpp Wed May 28 01:48:23 2014 +0000 @@ -18,8 +18,8 @@ while(true) { - pc.printf("frq: %f, vol: %f, amp: %f, rud: %f, dut: %f, pit: %f, t: %f\n", - mainCtrl.getFrequency(), mainCtrl.getVolume(), mainCtrl.getAmplitude(), mainCtrl.getRudder(), mainCtrl.getDutyCycle(), mainCtrl.getPitch(), t.read()); + pc.printf("frq: %f, vol: %f, amp: %f, yaw: %f, dut: %f, pit: %f, tadd: %f, t: %f\n", + mainCtrl.getFrequency(), mainCtrl.getVolume(), mainCtrl.getAmplitude(), mainCtrl.getYaw(), mainCtrl.getDutyCycle(), mainCtrl.getPitch(),mainCtrl.getTimeAdd(), t.read()); wait_ms(100); }