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 22:807d5467fbf6, committed 2014-06-05
- Comitter:
- sandwich
- Date:
- Thu Jun 05 00:13:02 2014 +0000
- Parent:
- 21:835fd919b4bd
- Child:
- 23:ef1be367726e
- Commit message:
- look at the previous one
Changed in this revision
--- a/MainController.cpp Tue Jun 03 15:30:09 2014 +0000 +++ b/MainController.cpp Thu Jun 05 00:13:02 2014 +0000 @@ -1,16 +1,19 @@ #include "MainController.h" MainController::MainController() - :ch1(p18,0.056,0.090), // yaw - ch2(p17,0.054,0.092), // pitch - ch3(p15,0.054,0.092), // amplitude - ch4(p30,0.055,0.092), // adj - ch6(p16,0.053,0.092), // frequency - mcon(p22, p6, p7), // change pin p5 to p7, because p5 is burned through + :ch1(p18,0.047,0.081), // yaw + ch2(p17,0.047,0.081), // pitch + ch3(p15,0.047,0.081), // amplitude + ch4(p30,0.047,0.081), // adj + ch6(p16,0.047,0.081), // frequency + ch5(p29,0.047,0.081), // manual vs auto control + syren(p9,p10), //syren is 38400bps serial //ap(p25, p26)//, leftservo(p21), - rightservo(p23) - + rightservo(p23), + pcam(p11,p12,p13,10), + controlThread(&MainController::controlThreadStarter, this, osPriorityRealtime, 1024), + trackerThread(&MainController::trackerThreadStarter, this, osPriorityHigh, 1024) { wait_ms(50); amp = 0.0; @@ -30,118 +33,177 @@ raiser = 0.0; pitAvg = 0.0; alPi = 0.2/(0.2+0.001);//tf/(tf+Ts); - - //leftservo.calibrate(0.0008, 45); - //rightservo.calibrate(-0.0001, 45); + syren.baud(38400); + //pcam=new pixySPI(p11,p12,p13,10); + + //leftservo.calibrate(0.0008, 45); + //rightservo.calibrate(-0.0001, 45); + +} +MainController::~MainController() +{ + //delete pcam; +} + +void MainController::controlThreadStarter(void const *p) +{ + MainController *instance = (MainController*)p; + instance->control(); +} + +void MainController::trackerThreadStarter(void const *p) +{ + MainController *instance = (MainController*)p; + instance->trackTarget(); } void MainController::control() { - curTime = timer1.read(); - - // 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.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 ) { - //read other new inputs - amp = this->calculateAmplitude(); // a value from 0 to 1 - frq = this->calculateFrequency(); // a value from frqmin to frqmax + controlThread.signal_wait(START_THREAD); + while (1) { + trackMutex.lock(); + curTime = timer1.read(); + //Block bestObject=pcam.getBlocks()[0]; //our best bet is the first block because it has the biggest area + + // check every half cycle + if(curTime > 1/(2*frqCmd) ) { + // read new yaw value every half cycle + if (ch5.dutycyclescaledup()<=0.5) + yaw = this->calculateYaw(); // a value from -1 to 1 + else { + //do proportional control on fish yaw + float gain=1/float(CENTER_X); + float error=pcam.getBestX()-CENTER_X; //calculate yaw difference + yaw=error*gain; + //printf("xlocation: %d -> %f\n", pcam.getBestX(), yaw); + } + + 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; - if(volChg > 0.0) { - // adjust frequency to add additional volume - if( amp < 0.0001 ) { - amp = 0.0001; - } - timeAdd = volChg/amp; + // 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( timeAdd > 0.5/frq ) { - timeAdd = 0.5/frq; - volChg = timeAdd * 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; - } - - fullCycle = false; + ampNew = amp; + + if(yaw < 0.0) { + ampNew = (1.0+0.7*yaw)*amp; + } + + fullCycle = false; - } else { - // reverse for the downward slope - amp = -amp; + } 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(volChg < 0.0) { - // adjust frequency to add additional volume - if( amp > -0.0001 ) { - amp = -0.0001; + if( timeAdd > 0.5/frq ) { + timeAdd = 0.5/frq; + volChg = timeAdd * amp; + } } - 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; } + + // use amp and frq from last cycle in order to make sure it is equalized + fullCycle = true; } - - ampNew = amp; - - if(yaw > 0.0) - { - ampNew = (1.0-0.7*yaw)*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) ) ); + + // 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 + + //reset timer + timer1.reset(); + curTime = 0.0; } - // update the frequency that actually needs to be commanded - frqCmd = 1.0/( 2.0*( timeAdd + 1/( 2* 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 - - //reset timer - timer1.reset(); - curTime = 0.0; + //Set Servo Values + if (ch5.dutycyclescaledup()<=0.5) + 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 error=pcam.getBestY()-CENTER_Y; + pitch=error*gain; + //printf("ylocation: %d -> %f\n", pcam.getBestY(), pitch); + } + leftservo = pitch+0.03; + rightservo = 1.0 - pitch; + 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 + + //now scale duty cycle from -1 to 1 -> 0 to 255 + dutyCycle=(255/2)*dutyCycle+127; + if (dutyCycle<0) + dutyCycle=0.0; + else if (dutyCycle>255) + dutyCycle=255.0; + + //mcon.setpolarspeed(dutyCycle); + syren.putc(int(dutyCycle)); + //syren.putc(int(0)); + trackMutex.unlock(); + Thread::wait(1); } - - - //Set Servo Values - pitch = this->calculatePitch(); - leftservo = pitch+0.03; - rightservo = 1.0 - pitch; - if (rightservo<0.03){ - rightservo = 0.03; +} + +void MainController::trackTarget() +{ + trackerThread.signal_wait(START_THREAD); + while (1) { + //trackMutex.lock(); + //printf("tracking\n"); + pcam.capture(); + //trackMutex.unlock(); } - - dutyCycle = ampNew * saturate(raiser * sin( 2.0 * MATH_PI * frqCmd * curTime )); // add factor 4.0 to get a cut off sinus - - mcon.setpolarspeed(dutyCycle); } float MainController::calculateFrequency() { - return ((frqMax - frqMin) * ch6.dutycyclescaledup() + frqMin); + return ((frqMax - frqMin) * ch6.dutycyclescaledup() + frqMin); } float MainController::calculateAmplitude() @@ -151,7 +213,7 @@ float MainController::calculateYaw() { - return (2.0*ch1.dutycyclescaledup() - 1.0); + return (2.0*ch1.dutycyclescaledup() - 1.0); } float MainController::calculatePitch() @@ -168,20 +230,27 @@ void MainController::start() { timer1.start(); - - ticker1.attach(this, &MainController::control,0.001); + + printf("start thread\n"); + controlThread.signal_set(START_THREAD); + trackerThread.signal_set(START_THREAD); + //ticker1.attach(this, &MainController::control,0.001); + //control=Thread(&MainController::control); + //tracker.attach(this, &MainController::trackTarget, 2.00); //update target position every second //Autopilot guardian //ap.calibrate(); //ap.set2D(); //ap.setoff(); - + } void MainController::stop() { timer1.stop(); - ticker1.detach(); - mcon.setpolarspeed(0.0); + //ticker1.detach(); + //tracker.detach(); + //mcon.setpolarspeed(0.0); + syren.putc(int(127)); } float MainController::getDutyCycle() @@ -225,6 +294,11 @@ return timeAdd; } +bool MainController::getOpMode() +{ + return ch5.dutycyclescaledup()>0.5; +} + //signum function float MainController::signum(float input) {
--- a/MainController.h Tue Jun 03 15:30:09 2014 +0000 +++ b/MainController.h Thu Jun 05 00:13:02 2014 +0000 @@ -2,15 +2,18 @@ #define MBED_MAINCONTROLLER_H #include "mbed.h" +#include "rtos.h" #include "PwmIn.h" #include "motor_controller.h" //#include "guardian.h" //#include "IMU.h" #include "Servo.h" +#include "pixy.h" #define MATH_PI 3.14159265359 +#define START_THREAD 1 /** MainController class to get control inputs and place them onto the system * @@ -23,6 +26,7 @@ * @param */ MainController() ; + ~MainController(); /** Start the main controller * @@ -38,6 +42,7 @@ float getPitch(); float getTimeAdd(); float getAdj(); + bool getOpMode(); //returns true when in autonomous mode /** Stop the main controller * @@ -48,6 +53,7 @@ protected: void control(); + void trackTarget(); float calculateFrequency(); float calculateAmplitude(); float calculateYaw(); @@ -62,13 +68,23 @@ PwmIn ch3; PwmIn ch4; PwmIn ch6; - PololuMController mcon; + PwmIn ch5; + //PololuMController mcon; + Serial syren; //syren replaced the pololu controller //Guardian ap; Servo leftservo; Servo rightservo; + pixySPI pcam; + Timer timer1; - Ticker ticker1; + //Ticker ticker1; + //Ticker tracker; + Mutex trackMutex; + Thread controlThread; + static void controlThreadStarter(void const *p); + Thread trackerThread; + static void trackerThreadStarter(void const *p); float amp; float ampNew; float frq;
--- a/PwmIn.cpp Tue Jun 03 15:30:09 2014 +0000 +++ b/PwmIn.cpp Thu Jun 05 00:13:02 2014 +0000 @@ -46,7 +46,7 @@ void PwmIn::fall() { _tmp = _t.read_us(); - if(_tmp > 1000 && _tmp < 1950 && _risen == true) { + if(_tmp > 500 && _tmp < 5000 && _risen == true) { _pulsewidth = _tmp; _risen = false; }
--- a/main.cpp Tue Jun 03 15:30:09 2014 +0000 +++ b/main.cpp Thu Jun 05 00:13:02 2014 +0000 @@ -4,26 +4,21 @@ //#include "PwmReader.h" //#include "PwmIn.h" #include "MainController.h" - Serial pc(USBTX, USBRX); Timer t; int main() { t.start(); - + MainController mainCtrl; - - // pixy cam! - pixySPI pcam(p5,p6,p7, 20, &pc); - // - + mainCtrl.start(); + //printf("started\n"); while(true) { - - pc.printf("frq: %.2f, vol: %.2f, amp: %.2f, yaw: %.2f, dut: %.2f, pit: %.2f, adj: %.2f, tadd: %.2f, t: %.2f\n", - 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, 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()); wait_ms(100); }
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Thu Jun 05 00:13:02 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-rtos/#015df9e602b6
--- a/pixy_cam.lib Tue Jun 03 15:30:09 2014 +0000 +++ b/pixy_cam.lib Thu Jun 05 00:13:02 2014 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/teams/jetfishteam/code/pixy_cam/#b3a6b5a898b4 +http://mbed.org/teams/jetfishteam/code/pixy_cam/#f54759b26096