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
- Committer:
- sandwich
- Date:
- 2014-07-11
- Revision:
- 25:4f2f441eceec
- Parent:
- 24:9d75ed1462d6
File content as of revision 25:4f2f441eceec:
#include "MainController.h" MainController::MainController() :ch1(p18,0.047,0.081), // yaw ch2(p17,0.047,0.09), // 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 //ap(p25, p26)//, leftservo(p21), rightservo(p24), pcam(p11,p12,p13,10), controlThread(&MainController::controlThreadStarter, this, osPriorityNormal, 1024), trackerThread(&MainController::trackerThreadStarter, this, osPriorityNormal, 1024), panLoop(300,100), //pgain, dgain tiltLoop(500,900) //pgain, dgain //controlTimer(&MainController::control, osTimerPeriodic, (void*) 0) { wait_ms(50); amp = 0.0; frq = 1.0; frqCmd = frq; yaw = 0.5; pitch = 0.5; adj = 0.5; frqMin = 0.8; //hardcoded frqMax = 2.9; //hardcoded //change = 0; //state = 0; fullCycle = true; volume = 0.0; volMax = 0.1; yawAdjVal = 0.7; raiser = 0.0; pitAvg = 0.0; alPi = 0.2/(0.2+0.001);//tf/(tf+Ts); syren.baud(9600); dutyCycle=127; //pcam=new pixySPI(p11,p12,p13,10); //leftservo.calibrate(0.0008, 45); //rightservo.calibrate(-0.0001, 45); } MainController::~MainController() { } /* void MainController::dutyThreadStarter(void const* p) { MainController *instance = (MainController*)p; instance->updateDutyCycle(); } */ 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::updateDutyCycle() { dutyThread.signal_wait(START_THREAD); while (1) { dutyMutex.lock(); syren.putc(int(dutyCycle)); dutyMutex.unlock(); } } */ void MainController::control() { controlThread.signal_wait(START_THREAD); while (1) { // //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 bool autonomous=getOpMode(); // check every half cycle if(curTime > 1/(2*frqCmd) ) { // read new yaw value every half cycle if (autonomous==false) { yaw = this->calculateYaw(); // a value from -1 to 1 if(yaw < 0.1 && yaw > -0.1) yaw =0.0; } else { trackMutex.lock(); int error=CENTER_X-pcam.getBestX(); //calculate yaw difference trackMutex.unlock(); panLoop.update(error); yaw=(float(panLoop.m_pos)/1000.0f)*2-1.00f; //scale from (0,1000) -> (-1,1) } // 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(yaw < 0.0) { ampCmd = (1.0+ yawAdjVal * yaw)*amp; } else { ampCmd = amp; } fullCycle = false; } else { // reverse for the downward slope amp = -amp; if(yaw > 0.0) { 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 = frq; // read new yaw value every half cycle adj = this->calculateAdj(); // a value from 0 to 1 // 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(); curTime = 0.0; } //Set Servo Values if (autonomous==false) pitch = this->calculatePitch(); else { trackMutex.lock(); int error=pcam.getBestY()-CENTER_Y; trackMutex.unlock(); tiltLoop.update(error); pitch=float(tiltLoop.m_pos)/1000.0f; //printf("Y error: %d, pitch: %f\n", error, 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) { rightservo = 0.03; } //dutyMutex.lock(); dutyCycle = ampCmd * 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; else if (dutyCycle>255) dutyCycle=255; syren.putc(uint16_t(dutyCycle)); //Thread::wait(1); //dutyMutex.unlock(); } } void MainController::trackTarget() { trackerThread.signal_wait(START_THREAD); while (1) { if (getOpMode()==true) { trackMutex.lock(); pcam.capture(); trackMutex.unlock(); } Thread::wait(100); } } float MainController::calculateFrequency() { return ((frqMax - frqMin) * ch6.dutycyclescaledup() + frqMin); } float MainController::calculateAmplitude() { return (ch3.dutycyclescaledup()); } float MainController::calculateYaw() { return (2.0*ch1.dutycyclescaledup() - 1.0); } float MainController::calculatePitch() { pitAvg = alPi * pitAvg+ (1.0 - alPi)*(ch2.dutycyclescaledup()); return pitAvg; } float MainController::calculateAdj() { return (ch4.dutycyclescaledup()); } void MainController::start() { timer1.start(); printf("start ticker\n"); //ticker1.attach(this, &MainController::control,0.001); /* while (1) { syren.putc(int(dutyCycle)); } */ controlThread.signal_set(START_THREAD); //dutyThread.signal_set(START_THREAD); trackerThread.signal_set(START_THREAD); //controlTimer.start(2); //control=Thread(&MainController::control); //tracker.attach(this, &MainController::trackTarget, 0.1); //update target position //Autopilot guardian //ap.calibrate(); //ap.set2D(); //ap.setoff(); //RtosTimer controlTimer((void*)(&control), osTimerPeriodic, (void *)0); //controlTimer.start(5); //while(true) { //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()); //Thread::wait(100); //wait_ms(100); } void MainController::stop() { timer1.stop(); //ticker1.detach(); //tracker.detach(); //mcon.setpolarspeed(0.0); syren.putc(int(127)); } float MainController::getDutyCycle() { return dutyCycle; } float MainController::getAmplitude() { return amp; } float MainController::getFrequency() { return frq; } float MainController::getVolume() { return volume; } float MainController::getYaw() { return yaw; } float MainController::getPitch() { return pitch; } float MainController::getAdj() { return adj; } bool MainController::getOpMode() { return ch5.dutycyclescaledup()>0.5; } //signum function float MainController::signum(float input) { if (input>0.0) return 1.0; else if (input<0.0) return (-1.0); else return 0.0; } //saturate function float MainController::saturate(float input) { if (input > 1.0) return (1.0); else if (input < -1.0) return (-1.0); else return input; }