jetfishteam / Mbed 2 deprecated jetfish

the fish that looks like a jet

Dependencies:   ADXL345 ADXL345_I2C IMUfilter ITG3200 mbed Servo

MainController.cpp@16:79cfe6201318, 2014-02-17 (annotated)

Committer:
rkk
Date:
Mon Feb 17 14:04:09 2014 +0000
Revision:
16:79cfe6201318
Parent:
15:dc5753a5b83e 
changed p5 to p7

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