jetfishteam / Mbed 2 deprecated jetfish

the fish that looks like a jet

Dependencies:   ADXL345 ADXL345_I2C IMUfilter ITG3200 mbed Servo

MainController.cpp@14:a5389e26a63b, 2014-02-04 (annotated)

Committer:
rkk
Date:
Tue Feb 04 14:23:56 2014 +0000
Revision:
14:a5389e26a63b
Parent:
13:5ed8fd870723
Child:
15:dc5753a5b83e
bang bang controller

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 13:5ed8fd870723 16 goofftime = 0.0;
rkk 13:5ed8fd870723 17 switched = false;
rkk 11:8ec915eb70f6 18 }
rkk 11:8ec915eb70f6 19
rkk 11:8ec915eb70f6 20 void MainController::control()
rkk 11:8ec915eb70f6 21 {
rkk 11:8ec915eb70f6 22 curTime = timer1.read();
rkk 11:8ec915eb70f6 23 if(curTime > 1/frq) {
rkk 11:8ec915eb70f6 24 //read new inputs
rkk 11:8ec915eb70f6 25 vol = this->calculateVolume();
rkk 11:8ec915eb70f6 26 frq = this->calculateFrequency();
rkk 13:5ed8fd870723 27 rud = this->calculateRudder(); //not used right now
rkk 11:8ec915eb70f6 28 timer1.reset();
rkk 11:8ec915eb70f6 29 curTime = 0.0;
rkk 13:5ed8fd870723 30 switched = false;
rkk 14:a5389e26a63b 31 goofftime = (vol/(2*frq));
rkk 14:a5389e26a63b 32 amplitude = vol;
rkk 13:5ed8fd870723 33
rkk 11:8ec915eb70f6 34 }
rkk 12:7eeb29892625 35
rkk 13:5ed8fd870723 36 //vol * sqrt(frq)/frqmxsqrt;
rkk 13:5ed8fd870723 37
rkk 14:a5389e26a63b 38 // if (curTime > 1/(2*frq) && (switched == false))
rkk 14:a5389e26a63b 39 // {
rkk 14:a5389e26a63b 40 // switched = true;
rkk 14:a5389e26a63b 41 // amplitude = -1.0;
rkk 14:a5389e26a63b 42 // }
rkk 14:a5389e26a63b 43 //
rkk 14:a5389e26a63b 44 // if(!switched)
rkk 14:a5389e26a63b 45 // {
rkk 14:a5389e26a63b 46 // if(curTime > goofftime )
rkk 14:a5389e26a63b 47 // amplitude = 0.0;
rkk 14:a5389e26a63b 48 // }
rkk 14:a5389e26a63b 49 // else
rkk 14:a5389e26a63b 50 // {
rkk 14:a5389e26a63b 51 // if(curTime > (1/(2*frq)+goofftime))
rkk 14:a5389e26a63b 52 // amplitude = 0.0;
rkk 14:a5389e26a63b 53 // }
rkk 13:5ed8fd870723 54
rkk 13:5ed8fd870723 55 dutyCycle = amplitude * signum(sin( 2.0 * MATH_PI * frq * curTime ));
rkk 13:5ed8fd870723 56
rkk 11:8ec915eb70f6 57 mcon.setpolarspeed(dutyCycle);
rkk 11:8ec915eb70f6 58 }
rkk 12:7eeb29892625 59
rkk 12:7eeb29892625 60
rkk 11:8ec915eb70f6 61 float MainController::calculateFrequency()
rkk 11:8ec915eb70f6 62 {
rkk 11:8ec915eb70f6 63 return ((frqMax - frqMin) * ch3.dutycyclescaledup() + frqMin);
rkk 11:8ec915eb70f6 64 }
rkk 11:8ec915eb70f6 65
rkk 11:8ec915eb70f6 66 float MainController::calculateVolume()
rkk 11:8ec915eb70f6 67 {
rkk 11:8ec915eb70f6 68 return (ch6.dutycyclescaledup());
rkk 11:8ec915eb70f6 69 }
rkk 11:8ec915eb70f6 70
rkk 12:7eeb29892625 71 float MainController::calculateRudder()
rkk 12:7eeb29892625 72 {
rkk 12:7eeb29892625 73 return (ch4.dutycyclescaledup());
rkk 12:7eeb29892625 74 }
rkk 12:7eeb29892625 75
rkk 11:8ec915eb70f6 76 void MainController::start()
rkk 11:8ec915eb70f6 77 {
rkk 11:8ec915eb70f6 78 timer1.start();
rkk 12:7eeb29892625 79 ticker1.attach(this, &MainController::control,0.001);
rkk 12:7eeb29892625 80 //Autopilot guardian
rkk 12:7eeb29892625 81 //ap.calibrate();
rkk 12:7eeb29892625 82 //ap.set2D();
rkk 12:7eeb29892625 83 ap.setoff();
rkk 12:7eeb29892625 84
rkk 11:8ec915eb70f6 85 }
rkk 11:8ec915eb70f6 86
rkk 11:8ec915eb70f6 87 void MainController::stop()
rkk 11:8ec915eb70f6 88 {
rkk 11:8ec915eb70f6 89 timer1.stop();
rkk 11:8ec915eb70f6 90 ticker1.detach();
rkk 11:8ec915eb70f6 91 mcon.setpolarspeed(0.0);
rkk 11:8ec915eb70f6 92 }
rkk 11:8ec915eb70f6 93
rkk 11:8ec915eb70f6 94 float MainController::getDutyCycle()
rkk 11:8ec915eb70f6 95 {
rkk 11:8ec915eb70f6 96 return dutyCycle;
rkk 11:8ec915eb70f6 97 }
rkk 11:8ec915eb70f6 98
rkk 11:8ec915eb70f6 99 float MainController::getAmplitude()
rkk 11:8ec915eb70f6 100 {
rkk 11:8ec915eb70f6 101 return amplitude;
rkk 11:8ec915eb70f6 102 }
rkk 11:8ec915eb70f6 103
rkk 11:8ec915eb70f6 104
rkk 11:8ec915eb70f6 105 float MainController::getFrequency()
rkk 11:8ec915eb70f6 106 {
rkk 11:8ec915eb70f6 107 return frq;
rkk 11:8ec915eb70f6 108 }
rkk 11:8ec915eb70f6 109
rkk 11:8ec915eb70f6 110 float MainController::getVolume()
rkk 11:8ec915eb70f6 111 {
rkk 11:8ec915eb70f6 112 return vol;
rkk 11:8ec915eb70f6 113 }
rkk 12:7eeb29892625 114
rkk 12:7eeb29892625 115 float MainController::getRudder()
rkk 12:7eeb29892625 116 {
rkk 12:7eeb29892625 117 return rud;
rkk 12:7eeb29892625 118 }
rkk 12:7eeb29892625 119
rkk 12:7eeb29892625 120 //
rkk 13:5ed8fd870723 121 float MainController::signum(float input)
rkk 13:5ed8fd870723 122 {
rkk 13:5ed8fd870723 123 if (input>0.0)
rkk 14:a5389e26a63b 124 return 1.0;
rkk 13:5ed8fd870723 125 else if (input<0.0)
rkk 14:a5389e26a63b 126 return (-1.0);
rkk 13:5ed8fd870723 127 else
rkk 14:a5389e26a63b 128 return 0.0;
rkk 13:5ed8fd870723 129 }