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 23:ef1be367726e, committed 2014-06-05
- Comitter:
- sandwich
- Date:
- Thu Jun 05 17:32:51 2014 +0000
- Parent:
- 22:807d5467fbf6
- Child:
- 24:9d75ed1462d6
- Commit message:
- -has tele-op and autonomous mode; -left slider now controls gain on pitch; -works with syren now; -uses rtos thread model for better multitasking
Changed in this revision
--- a/MainController.cpp Thu Jun 05 00:13:02 2014 +0000 +++ b/MainController.cpp Thu Jun 05 17:32:51 2014 +0000 @@ -29,7 +29,9 @@ fullCycle = true; volume = 0.0; volMax = 0.1; - volChg = 0.0; + + yawAdjVal = 0.7; + raiser = 0.0; pitAvg = 0.0; alPi = 0.2/(0.2+0.001);//tf/(tf+Ts); @@ -69,24 +71,18 @@ // check every half cycle if(curTime > 1/(2*frqCmd) ) { // read new yaw value every half cycle - if (ch5.dutycyclescaledup()<=0.5) + if (ch5.dutycyclescaledup()<=0.5) { yaw = this->calculateYaw(); // a value from -1 to 1 - else { + if(yaw < 0.1 && yaw > -0.1) + yaw =0.0; + } else { //do proportional control on fish yaw - float gain=1/float(CENTER_X); + float gain=(1/(1+ampCmd))*frqCmd*(1/float(CENTER_X)); float error=pcam.getBestX()-CENTER_X; //calculate yaw difference - yaw=error*gain; - //printf("xlocation: %d -> %f\n", pcam.getBestX(), yaw); + yaw=error*gain+0.5f; + //printf("x: %d, ", pcam.getBestX()); } - if(yaw < 0.1 && yaw > -0.1) { - 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 ) { @@ -94,22 +90,10 @@ 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; - } - } - ampNew = amp; - if(yaw < 0.0) { - ampNew = (1.0+0.7*yaw)*amp; + ampCmd = (1.0+ yawAdjVal * yaw)*amp; + } else { + ampCmd = amp; } fullCycle = false; @@ -118,38 +102,27 @@ // 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; - } - } - - ampNew = amp; if(yaw > 0.0) { - ampNew = (1.0-0.7*yaw)*amp; + ampCmd = (1.0- yawAdjVal *yaw)*amp; + } else { + ampCmd = amp; } // 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) ) ); + frqCmd = frq; // read new yaw value every half cycle adj = this->calculateAdj(); // a value from 0 to 1 - // 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 - raiser = ( 5 * adj + 1.0); // varied from 1 to 5 + + // adj value used to define the trapezoid shape + raiser = ( 5 * 0.7 + 1.0); // varied from 1 to 5 for now hard coded //reset timer timer1.reset(); @@ -162,18 +135,28 @@ pitch = this->calculatePitch(); else { //do a proportional control on the error since servos already give you position control - float gain=1/float(CENTER_Y); //try this out for now + float gain=(1/(1+abs(ampCmd)))*(1/frqCmd)*(1/float(CENTER_Y))*adj; //try this out for now + //float gain=(1/float(CENTER_Y))*adj; float error=pcam.getBestY()-CENTER_Y; - pitch=error*gain; - //printf("ylocation: %d -> %f\n", pcam.getBestY(), pitch); + pitch=error*gain+0.5; + //printf("error: %f\n", error); + //printf("y: %d\n", pcam.getBestY()); + //printf("pitch: %f\n", pitch); } + + //Adjusting the trim of the pitch angles leftservo = pitch+0.03; + if (leftservo > 1.0) { + leftservo = 1.0; + } rightservo = 1.0 - pitch; - if (rightservo<0.03) { + if (rightservo < 0.03) { rightservo = 0.03; } - dutyCycle = ampNew * saturate(raiser * sin( 2.0 * MATH_PI * frqCmd * curTime )); // add factor 4.0 to get a cut off sinus + + dutyCycle = ampCmd * saturate(raiser * sin( 2.0 * MATH_PI * frqCmd * curTime )); // add factor 4.0 to get a cut off sinus + //printf("dc: %f\n", dutyCycle); //now scale duty cycle from -1 to 1 -> 0 to 255 dutyCycle=(255/2)*dutyCycle+127; @@ -183,6 +166,7 @@ dutyCycle=255.0; //mcon.setpolarspeed(dutyCycle); + //dutyCycle=100; syren.putc(int(dutyCycle)); //syren.putc(int(0)); trackMutex.unlock(); @@ -289,11 +273,6 @@ return adj; } -float MainController::getTimeAdd() -{ - return timeAdd; -} - bool MainController::getOpMode() { return ch5.dutycyclescaledup()>0.5;
--- a/MainController.h Thu Jun 05 00:13:02 2014 +0000 +++ b/MainController.h Thu Jun 05 17:32:51 2014 +0000 @@ -40,7 +40,6 @@ float getAmplitude(); float getYaw(); float getPitch(); - float getTimeAdd(); float getAdj(); bool getOpMode(); //returns true when in autonomous mode @@ -86,7 +85,7 @@ Thread trackerThread; static void trackerThreadStarter(void const *p); float amp; - float ampNew; + float ampCmd; float frq; float dutyCycle; float curTime; @@ -95,13 +94,11 @@ float yaw; float pitch; float adj; - + float yawAdjVal; bool fullCycle; float volume; - float volChg; float volMax; float frqCmd; - float timeAdd; float raiser; float pitAvg; float alPi;
--- a/main.cpp Thu Jun 05 00:13:02 2014 +0000 +++ b/main.cpp Thu Jun 05 17:32:51 2014 +0000 @@ -17,10 +17,10 @@ //printf("started\n"); while(true) { - pc.printf("control: %d, frq: %.3f, vol: %.2f, amp: %.3f, yaw: %.2f, dut: %.2f, pit: %.2f, adj: %.2f, tadd: %.2f, t: %.2f\n", - mainCtrl.getOpMode(), mainCtrl.getFrequency(), mainCtrl.getVolume(), mainCtrl.getAmplitude(), mainCtrl.getYaw(), mainCtrl.getDutyCycle(), mainCtrl.getPitch(), mainCtrl.getAdj(), mainCtrl.getTimeAdd(), t.read()); + pc.printf("control: %d, frq: %.3f, vol: %.2f, amp: %.3f, yaw: %.2f, dut: %.2f, pit: %.2f, adj: %.2f, t: %.2f\n", + mainCtrl.getOpMode(), mainCtrl.getFrequency(), mainCtrl.getVolume(), mainCtrl.getAmplitude(), mainCtrl.getYaw(), mainCtrl.getDutyCycle(), mainCtrl.getPitch(), mainCtrl.getAdj(), t.read()); - wait_ms(100); + //wait_ms(100); } //t.stop(); //mainCtrl.stop();
--- a/pixy_cam.lib Thu Jun 05 00:13:02 2014 +0000 +++ b/pixy_cam.lib Thu Jun 05 17:32:51 2014 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/teams/jetfishteam/code/pixy_cam/#f54759b26096 +http://mbed.org/teams/jetfishteam/code/pixy_cam/#4bf8d39fc5ce