jetfishteam / Mbed 2 deprecated jetfish

the fish that looks like a jet

Dependencies:   ADXL345 ADXL345_I2C IMUfilter ITG3200 mbed Servo

MainController.cpp@12:7eeb29892625, 2014-02-01 (annotated)

Committer:
rkk
Date:
Sat Feb 01 00:03:40 2014 +0000
Revision:
12:7eeb29892625
Parent:
11:8ec915eb70f6
Child:
13:5ed8fd870723
first design to put to water

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rkk 11:8ec915eb70f6 1 #include "MainController.h"
rkk 11:8ec915eb70f6 2
rkk 11:8ec915eb70f6 3 MainController::MainController()
rkk 12:7eeb29892625 4 :ch3(p16,0.054,0.092), // frequency
rkk 12:7eeb29892625 5 ch4(p17,0.055,0.092), //rudder
rkk 12:7eeb29892625 6 ch6(p15,0.055,0.092), //volume
rkk 12:7eeb29892625 7 mcon(p22, p6, p5),
rkk 12:7eeb29892625 8 ap(p25, p26)
rkk 11:8ec915eb70f6 9 {
rkk 12:7eeb29892625 10 wait_ms(50);
rkk 11:8ec915eb70f6 11 vol = 0.0;
rkk 11:8ec915eb70f6 12 frq = 10.0;
rkk 12:7eeb29892625 13 rud = 0.5;
rkk 11:8ec915eb70f6 14 frqMin = 0.5; // hardcoded
rkk 11:8ec915eb70f6 15 frqMax = 3.0; //hardcoded
rkk 12:7eeb29892625 16 frqmxsqrt = sqrt(frqMax);
rkk 11:8ec915eb70f6 17 }
rkk 11:8ec915eb70f6 18
rkk 11:8ec915eb70f6 19 void MainController::control()
rkk 11:8ec915eb70f6 20 {
rkk 11:8ec915eb70f6 21 curTime = timer1.read();
rkk 11:8ec915eb70f6 22 if(curTime > 1/frq) {
rkk 11:8ec915eb70f6 23 //read new inputs
rkk 11:8ec915eb70f6 24 vol = this->calculateVolume();
rkk 11:8ec915eb70f6 25 frq = this->calculateFrequency();
rkk 11:8ec915eb70f6 26 timer1.reset();
rkk 11:8ec915eb70f6 27 curTime = 0.0;
rkk 11:8ec915eb70f6 28 }
rkk 12:7eeb29892625 29
rkk 12:7eeb29892625 30 rud = this->calculateRudder(); //not used right now
rkk 12:7eeb29892625 31 amplitude = vol * sqrt(frq)/frqmxsqrt;
rkk 11:8ec915eb70f6 32 dutyCycle = amplitude * sin( 2.0 * MATH_PI * frq * curTime );
rkk 12:7eeb29892625 33 //sigm for alternative driving mode
rkk 11:8ec915eb70f6 34 mcon.setpolarspeed(dutyCycle);
rkk 11:8ec915eb70f6 35 }
rkk 12:7eeb29892625 36
rkk 12:7eeb29892625 37
rkk 11:8ec915eb70f6 38 float MainController::calculateFrequency()
rkk 11:8ec915eb70f6 39 {
rkk 11:8ec915eb70f6 40 return ((frqMax - frqMin) * ch3.dutycyclescaledup() + frqMin);
rkk 11:8ec915eb70f6 41 }
rkk 11:8ec915eb70f6 42
rkk 11:8ec915eb70f6 43 float MainController::calculateVolume()
rkk 11:8ec915eb70f6 44 {
rkk 11:8ec915eb70f6 45 return (ch6.dutycyclescaledup());
rkk 11:8ec915eb70f6 46 }
rkk 11:8ec915eb70f6 47
rkk 12:7eeb29892625 48 float MainController::calculateRudder()
rkk 12:7eeb29892625 49 {
rkk 12:7eeb29892625 50 return (ch4.dutycyclescaledup());
rkk 12:7eeb29892625 51 }
rkk 12:7eeb29892625 52
rkk 11:8ec915eb70f6 53 void MainController::start()
rkk 11:8ec915eb70f6 54 {
rkk 11:8ec915eb70f6 55 timer1.start();
rkk 12:7eeb29892625 56 ticker1.attach(this, &MainController::control,0.001);
rkk 12:7eeb29892625 57 //Autopilot guardian
rkk 12:7eeb29892625 58 //ap.calibrate();
rkk 12:7eeb29892625 59 //ap.set2D();
rkk 12:7eeb29892625 60 ap.setoff();
rkk 12:7eeb29892625 61
rkk 11:8ec915eb70f6 62 }
rkk 11:8ec915eb70f6 63
rkk 11:8ec915eb70f6 64 void MainController::stop()
rkk 11:8ec915eb70f6 65 {
rkk 11:8ec915eb70f6 66 timer1.stop();
rkk 11:8ec915eb70f6 67 ticker1.detach();
rkk 11:8ec915eb70f6 68 mcon.setpolarspeed(0.0);
rkk 11:8ec915eb70f6 69 }
rkk 11:8ec915eb70f6 70
rkk 11:8ec915eb70f6 71 float MainController::getDutyCycle()
rkk 11:8ec915eb70f6 72 {
rkk 11:8ec915eb70f6 73 return dutyCycle;
rkk 11:8ec915eb70f6 74 }
rkk 11:8ec915eb70f6 75
rkk 11:8ec915eb70f6 76 float MainController::getAmplitude()
rkk 11:8ec915eb70f6 77 {
rkk 11:8ec915eb70f6 78 return amplitude;
rkk 11:8ec915eb70f6 79 }
rkk 11:8ec915eb70f6 80
rkk 11:8ec915eb70f6 81
rkk 11:8ec915eb70f6 82 float MainController::getFrequency()
rkk 11:8ec915eb70f6 83 {
rkk 11:8ec915eb70f6 84 return frq;
rkk 11:8ec915eb70f6 85 }
rkk 11:8ec915eb70f6 86
rkk 11:8ec915eb70f6 87 float MainController::getVolume()
rkk 11:8ec915eb70f6 88 {
rkk 11:8ec915eb70f6 89 return vol;
rkk 11:8ec915eb70f6 90 }
rkk 12:7eeb29892625 91
rkk 12:7eeb29892625 92 float MainController::getRudder()
rkk 12:7eeb29892625 93 {
rkk 12:7eeb29892625 94 return rud;
rkk 12:7eeb29892625 95 }
rkk 12:7eeb29892625 96
rkk 12:7eeb29892625 97 //
rkk 12:7eeb29892625 98 //float MainController::sigm(float input)
rkk 12:7eeb29892625 99 //{
rkk 12:7eeb29892625 100 // if (input>0)
rkk 12:7eeb29892625 101 // return 1;
rkk 12:7eeb29892625 102 // else if (input<0)
rkk 12:7eeb29892625 103 // return -1;
rkk 12:7eeb29892625 104 // else
rkk 12:7eeb29892625 105 // return 0;
rkk 12:7eeb29892625 106 //}