the fish that looks like a jet
Dependencies: ADXL345 ADXL345_I2C IMUfilter ITG3200 mbed Servo
MainController.cpp
- Committer:
- rkk
- Date:
- 2014-02-03
- Revision:
- 13:5ed8fd870723
- Parent:
- 12:7eeb29892625
- Child:
- 14:a5389e26a63b
File content as of revision 13:5ed8fd870723:
#include "MainController.h" MainController::MainController() :ch3(p16,0.054,0.092), // frequency ch4(p17,0.055,0.092), //rudder ch6(p15,0.055,0.092), //volume mcon(p22, p6, p5), ap(p25, p26) { wait_ms(50); vol = 0.0; frq = 10.0; rud = 0.5; frqMin = 0.5; // hardcoded frqMax = 3.0; //hardcoded goofftime = 0.0; switched = false; } void MainController::control() { curTime = timer1.read(); if(curTime > 1/frq) { //read new inputs vol = this->calculateVolume(); frq = this->calculateFrequency(); rud = this->calculateRudder(); //not used right now timer1.reset(); curTime = 0.0; switched = false; goofftime = (1/(2*frq)*vol); amplitude = 1.0; } //vol * sqrt(frq)/frqmxsqrt; if (curTime > 1/(2*frq)) { switched = true; amplitude = -1.0; } if(!switched) { if(curTime > goofftime ) amplitude = 0.0; } else { if(curTime > (1/(2*frq)+goofftime)) amplitude = 0.0; } dutyCycle = amplitude * signum(sin( 2.0 * MATH_PI * frq * curTime )); mcon.setpolarspeed(dutyCycle); } float MainController::calculateFrequency() { return ((frqMax - frqMin) * ch3.dutycyclescaledup() + frqMin); } float MainController::calculateVolume() { return (ch6.dutycyclescaledup()); } float MainController::calculateRudder() { return (ch4.dutycyclescaledup()); } void MainController::start() { timer1.start(); ticker1.attach(this, &MainController::control,0.001); //Autopilot guardian //ap.calibrate(); //ap.set2D(); ap.setoff(); } void MainController::stop() { timer1.stop(); ticker1.detach(); mcon.setpolarspeed(0.0); } float MainController::getDutyCycle() { return dutyCycle; } float MainController::getAmplitude() { return amplitude; } float MainController::getFrequency() { return frq; } float MainController::getVolume() { return vol; } float MainController::getRudder() { return rud; } // float MainController::signum(float input) { if (input>0.0) return 1; else if (input<0.0) return -1; else return 0; }