jetfishteam / Mbed 2 deprecated jetfish

the fish that looks like a jet

Dependencies:   ADXL345 ADXL345_I2C IMUfilter ITG3200 mbed Servo

MainController.cpp@13:5ed8fd870723, 2014-02-03 (annotated)

Committer:
rkk
Date:
Mon Feb 03 21:04:35 2014 +0000
Revision:
13:5ed8fd870723
Parent:
12:7eeb29892625
Child:
14:a5389e26a63b
change control approach to have bang bang and impulse length defining

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 13:5ed8fd870723 31 goofftime = (1/(2*frq)*vol);
rkk 13:5ed8fd870723 32 amplitude = 1.0;
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 13:5ed8fd870723 38 if (curTime > 1/(2*frq))
rkk 13:5ed8fd870723 39 {
rkk 13:5ed8fd870723 40 switched = true;
rkk 13:5ed8fd870723 41 amplitude = -1.0;
rkk 13:5ed8fd870723 42 }
rkk 13:5ed8fd870723 43
rkk 13:5ed8fd870723 44 if(!switched)
rkk 13:5ed8fd870723 45 {
rkk 13:5ed8fd870723 46 if(curTime > goofftime )
rkk 13:5ed8fd870723 47 amplitude = 0.0;
rkk 13:5ed8fd870723 48 }
rkk 13:5ed8fd870723 49 else
rkk 13:5ed8fd870723 50 {
rkk 13:5ed8fd870723 51 if(curTime > (1/(2*frq)+goofftime))
rkk 13:5ed8fd870723 52 amplitude = 0.0;
rkk 13:5ed8fd870723 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 13:5ed8fd870723 124 return 1;
rkk 13:5ed8fd870723 125 else if (input<0.0)
rkk 13:5ed8fd870723 126 return -1;
rkk 13:5ed8fd870723 127 else
rkk 13:5ed8fd870723 128 return 0;
rkk 13:5ed8fd870723 129 }