jetfishteam / Mbed 2 deprecated robotic_fish_ver_4_9_pixy

My modifications/additions to the code

Dependencies:   ADXL345 ADXL345_I2C IMUfilter ITG3200 Servo fishgait mbed-rtos mbed pixy_cam

Fork of robotic_fish_ver_4_8 by jetfishteam

MainController.cpp@20:6ae16da1492a, 2014-05-30 (annotated)

Committer:
rkk
Date:
Fri May 30 22:18:39 2014 +0000
Revision:
20:6ae16da1492a
Parent:
19:655db88b045c
Child:
22:807d5467fbf6
working setup for yaw motion

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 20:6ae16da1492a 4 :ch1(p18,0.056,0.090), // yaw
rkk 20:6ae16da1492a 5 ch2(p17,0.054,0.092), // pitch
rkk 20:6ae16da1492a 6 ch3(p15,0.054,0.092), // amplitude
rkk 20:6ae16da1492a 7 ch4(p30,0.055,0.092), // adj
rkk 20:6ae16da1492a 8 ch6(p16,0.053,0.092), // frequency
rkk 17:6970aef8154b 9 mcon(p22, p6, p7), // change pin p5 to p7, because p5 is burned through
rkk 20:6ae16da1492a 10 //ap(p25, p26)//,
rkk 20:6ae16da1492a 11 leftservo(p21),
rkk 20:6ae16da1492a 12 rightservo(p23)
rkk 17:6970aef8154b 13
rkk 11:8ec915eb70f6 14 {
rkk 12:7eeb29892625 15 wait_ms(50);
rkk 19:655db88b045c 16 amp = 0.0;
rkk 17:6970aef8154b 17 frq = 1.0;
rkk 19:655db88b045c 18 frqCmd = frq;
rkk 19:655db88b045c 19 yaw = 0.5;
rkk 17:6970aef8154b 20 pitch = 0.5;
rkk 20:6ae16da1492a 21 adj = 0.5;
rkk 20:6ae16da1492a 22 frqMin = 0.8; //hardcoded
rkk 20:6ae16da1492a 23 frqMax = 2.9; //hardcoded
rkk 19:655db88b045c 24 //change = 0;
rkk 19:655db88b045c 25 //state = 0;
rkk 19:655db88b045c 26 fullCycle = true;
rkk 19:655db88b045c 27 volume = 0.0;
rkk 19:655db88b045c 28 volMax = 0.1;
rkk 19:655db88b045c 29 volChg = 0.0;
rkk 20:6ae16da1492a 30 raiser = 0.0;
rkk 20:6ae16da1492a 31 pitAvg = 0.0;
rkk 20:6ae16da1492a 32 alPi = 0.2/(0.2+0.001);//tf/(tf+Ts);
rkk 20:6ae16da1492a 33
rkk 20:6ae16da1492a 34 //leftservo.calibrate(0.0008, 45);
rkk 20:6ae16da1492a 35 //rightservo.calibrate(-0.0001, 45);
rkk 20:6ae16da1492a 36
rkk 11:8ec915eb70f6 37 }
rkk 11:8ec915eb70f6 38
rkk 11:8ec915eb70f6 39 void MainController::control()
rkk 11:8ec915eb70f6 40 {
rkk 11:8ec915eb70f6 41 curTime = timer1.read();
rkk 15:dc5753a5b83e 42
rkk 19:655db88b045c 43 // check every half cycle
rkk 19:655db88b045c 44 if(curTime > 1/(2*frqCmd) ) {
rkk 19:655db88b045c 45
rkk 19:655db88b045c 46 // read new yaw value every half cycle
rkk 19:655db88b045c 47 yaw = this->calculateYaw(); // a value from -1 to 1
rkk 19:655db88b045c 48
rkk 20:6ae16da1492a 49 if(yaw < 0.1 && yaw > -0.1){
rkk 19:655db88b045c 50 yaw =0.0;
rkk 19:655db88b045c 51 }
rkk 19:655db88b045c 52 // calculate liquid volume change in the chambers
rkk 19:655db88b045c 53 volChg = volMax * yaw;
rkk 20:6ae16da1492a 54 volChg = 0.0;
rkk 19:655db88b045c 55
rkk 19:655db88b045c 56 timeAdd = 0.0;
rkk 19:655db88b045c 57
rkk 19:655db88b045c 58 // Read volume and frequency only every full cycle
rkk 19:655db88b045c 59 if( fullCycle ) {
rkk 19:655db88b045c 60 //read other new inputs
rkk 19:655db88b045c 61 amp = this->calculateAmplitude(); // a value from 0 to 1
rkk 19:655db88b045c 62 frq = this->calculateFrequency(); // a value from frqmin to frqmax
rkk 19:655db88b045c 63
rkk 19:655db88b045c 64 if(volChg > 0.0) {
rkk 19:655db88b045c 65 // adjust frequency to add additional volume
rkk 19:655db88b045c 66 if( amp < 0.0001 ) {
rkk 19:655db88b045c 67 amp = 0.0001;
rkk 19:655db88b045c 68 }
rkk 19:655db88b045c 69 timeAdd = volChg/amp;
rkk 19:655db88b045c 70
rkk 19:655db88b045c 71 if( timeAdd > 0.5/frq ) {
rkk 19:655db88b045c 72 timeAdd = 0.5/frq;
rkk 19:655db88b045c 73 volChg = timeAdd * amp;
rkk 19:655db88b045c 74 }
rkk 19:655db88b045c 75 }
rkk 20:6ae16da1492a 76 ampNew = amp;
rkk 19:655db88b045c 77
rkk 20:6ae16da1492a 78 if(yaw < 0.0)
rkk 20:6ae16da1492a 79 {
rkk 20:6ae16da1492a 80 ampNew = (1.0+0.7*yaw)*amp;
rkk 20:6ae16da1492a 81 }
rkk 19:655db88b045c 82
rkk 19:655db88b045c 83 fullCycle = false;
rkk 19:655db88b045c 84
rkk 19:655db88b045c 85 } else {
rkk 19:655db88b045c 86 // reverse for the downward slope
rkk 19:655db88b045c 87 amp = -amp;
rkk 19:655db88b045c 88
rkk 19:655db88b045c 89 if(volChg < 0.0) {
rkk 19:655db88b045c 90 // adjust frequency to add additional volume
rkk 19:655db88b045c 91 if( amp > -0.0001 ) {
rkk 19:655db88b045c 92 amp = -0.0001;
rkk 19:655db88b045c 93 }
rkk 19:655db88b045c 94 timeAdd = volChg/amp;
rkk 19:655db88b045c 95
rkk 19:655db88b045c 96 if( timeAdd > 0.5/frq ) {
rkk 19:655db88b045c 97 timeAdd = 0.5/frq;
rkk 19:655db88b045c 98 volChg = timeAdd * amp;
rkk 19:655db88b045c 99 }
rkk 19:655db88b045c 100 }
rkk 20:6ae16da1492a 101
rkk 20:6ae16da1492a 102 ampNew = amp;
rkk 20:6ae16da1492a 103
rkk 20:6ae16da1492a 104 if(yaw > 0.0)
rkk 20:6ae16da1492a 105 {
rkk 20:6ae16da1492a 106 ampNew = (1.0-0.7*yaw)*amp;
rkk 20:6ae16da1492a 107 }
rkk 19:655db88b045c 108
rkk 19:655db88b045c 109 // use amp and frq from last cycle in order to make sure it is equalized
rkk 19:655db88b045c 110 fullCycle = true;
rkk 19:655db88b045c 111 }
rkk 19:655db88b045c 112 // update the frequency that actually needs to be commanded
rkk 19:655db88b045c 113 frqCmd = 1.0/( 2.0*( timeAdd + 1/( 2* frq) ) );
rkk 19:655db88b045c 114
rkk 20:6ae16da1492a 115 // read new yaw value every half cycle
rkk 20:6ae16da1492a 116 adj = this->calculateAdj(); // a value from 0 to 1
rkk 20:6ae16da1492a 117
rkk 19:655db88b045c 118 // for keeping track, calculate current volume storage, positive means on side is fuller, negative means other side is fuller
rkk 19:655db88b045c 119 //volume = volChg + volume;
rkk 19:655db88b045c 120 // rudder value used to define the trapezoid shape
rkk 20:6ae16da1492a 121 raiser = ( 5 * adj + 1.0); // varied from 1 to 5
rkk 19:655db88b045c 122
rkk 19:655db88b045c 123 //reset timer
rkk 11:8ec915eb70f6 124 timer1.reset();
rkk 11:8ec915eb70f6 125 curTime = 0.0;
rkk 19:655db88b045c 126 }
rkk 15:dc5753a5b83e 127
rkk 19:655db88b045c 128
rkk 17:6970aef8154b 129 //Set Servo Values
rkk 20:6ae16da1492a 130 pitch = this->calculatePitch();
rkk 20:6ae16da1492a 131 leftservo = pitch+0.03;
rkk 20:6ae16da1492a 132 rightservo = 1.0 - pitch;
rkk 20:6ae16da1492a 133 if (rightservo<0.03){
rkk 20:6ae16da1492a 134 rightservo = 0.03;
rkk 20:6ae16da1492a 135 }
rkk 20:6ae16da1492a 136
rkk 20:6ae16da1492a 137 dutyCycle = ampNew * saturate(raiser * sin( 2.0 * MATH_PI * frqCmd * curTime )); // add factor 4.0 to get a cut off sinus
rkk 19:655db88b045c 138
rkk 11:8ec915eb70f6 139 mcon.setpolarspeed(dutyCycle);
rkk 11:8ec915eb70f6 140 }
rkk 12:7eeb29892625 141
rkk 11:8ec915eb70f6 142 float MainController::calculateFrequency()
rkk 11:8ec915eb70f6 143 {
rkk 20:6ae16da1492a 144 return ((frqMax - frqMin) * ch6.dutycyclescaledup() + frqMin);
rkk 11:8ec915eb70f6 145 }
rkk 11:8ec915eb70f6 146
rkk 19:655db88b045c 147 float MainController::calculateAmplitude()
rkk 11:8ec915eb70f6 148 {
rkk 20:6ae16da1492a 149 return (ch3.dutycyclescaledup());
rkk 11:8ec915eb70f6 150 }
rkk 11:8ec915eb70f6 151
rkk 19:655db88b045c 152 float MainController::calculateYaw()
rkk 12:7eeb29892625 153 {
rkk 20:6ae16da1492a 154 return (2.0*ch1.dutycyclescaledup() - 1.0);
rkk 12:7eeb29892625 155 }
rkk 12:7eeb29892625 156
rkk 17:6970aef8154b 157 float MainController::calculatePitch()
rkk 17:6970aef8154b 158 {
rkk 20:6ae16da1492a 159 pitAvg = alPi * pitAvg+ (1.0 - alPi)*(ch2.dutycyclescaledup());
rkk 20:6ae16da1492a 160 return pitAvg;
rkk 20:6ae16da1492a 161 }
rkk 20:6ae16da1492a 162
rkk 20:6ae16da1492a 163 float MainController::calculateAdj()
rkk 20:6ae16da1492a 164 {
rkk 20:6ae16da1492a 165 return (ch4.dutycyclescaledup());
rkk 17:6970aef8154b 166 }
rkk 17:6970aef8154b 167
rkk 11:8ec915eb70f6 168 void MainController::start()
rkk 11:8ec915eb70f6 169 {
rkk 11:8ec915eb70f6 170 timer1.start();
rkk 17:6970aef8154b 171
rkk 12:7eeb29892625 172 ticker1.attach(this, &MainController::control,0.001);
rkk 12:7eeb29892625 173 //Autopilot guardian
rkk 12:7eeb29892625 174 //ap.calibrate();
rkk 12:7eeb29892625 175 //ap.set2D();
rkk 20:6ae16da1492a 176 //ap.setoff();
rkk 12:7eeb29892625 177
rkk 11:8ec915eb70f6 178 }
rkk 11:8ec915eb70f6 179
rkk 11:8ec915eb70f6 180 void MainController::stop()
rkk 11:8ec915eb70f6 181 {
rkk 11:8ec915eb70f6 182 timer1.stop();
rkk 11:8ec915eb70f6 183 ticker1.detach();
rkk 11:8ec915eb70f6 184 mcon.setpolarspeed(0.0);
rkk 11:8ec915eb70f6 185 }
rkk 11:8ec915eb70f6 186
rkk 11:8ec915eb70f6 187 float MainController::getDutyCycle()
rkk 11:8ec915eb70f6 188 {
rkk 11:8ec915eb70f6 189 return dutyCycle;
rkk 11:8ec915eb70f6 190 }
rkk 11:8ec915eb70f6 191
rkk 11:8ec915eb70f6 192 float MainController::getAmplitude()
rkk 11:8ec915eb70f6 193 {
rkk 19:655db88b045c 194 return amp;
rkk 11:8ec915eb70f6 195 }
rkk 11:8ec915eb70f6 196
rkk 11:8ec915eb70f6 197
rkk 11:8ec915eb70f6 198 float MainController::getFrequency()
rkk 11:8ec915eb70f6 199 {
rkk 11:8ec915eb70f6 200 return frq;
rkk 11:8ec915eb70f6 201 }
rkk 11:8ec915eb70f6 202
rkk 11:8ec915eb70f6 203 float MainController::getVolume()
rkk 11:8ec915eb70f6 204 {
rkk 19:655db88b045c 205 return volume;
rkk 11:8ec915eb70f6 206 }
rkk 12:7eeb29892625 207
rkk 19:655db88b045c 208 float MainController::getYaw()
rkk 12:7eeb29892625 209 {
rkk 19:655db88b045c 210 return yaw;
rkk 12:7eeb29892625 211 }
rkk 12:7eeb29892625 212
rkk 17:6970aef8154b 213 float MainController::getPitch()
rkk 17:6970aef8154b 214 {
rkk 17:6970aef8154b 215 return pitch;
rkk 17:6970aef8154b 216 }
rkk 17:6970aef8154b 217
rkk 20:6ae16da1492a 218 float MainController::getAdj()
rkk 20:6ae16da1492a 219 {
rkk 20:6ae16da1492a 220 return adj;
rkk 20:6ae16da1492a 221 }
rkk 20:6ae16da1492a 222
rkk 19:655db88b045c 223 float MainController::getTimeAdd()
rkk 19:655db88b045c 224 {
rkk 19:655db88b045c 225 return timeAdd;
rkk 19:655db88b045c 226 }
rkk 19:655db88b045c 227
rkk 15:dc5753a5b83e 228 //signum function
rkk 13:5ed8fd870723 229 float MainController::signum(float input)
rkk 13:5ed8fd870723 230 {
rkk 13:5ed8fd870723 231 if (input>0.0)
rkk 14:a5389e26a63b 232 return 1.0;
rkk 13:5ed8fd870723 233 else if (input<0.0)
rkk 14:a5389e26a63b 234 return (-1.0);
rkk 13:5ed8fd870723 235 else
rkk 14:a5389e26a63b 236 return 0.0;
rkk 15:dc5753a5b83e 237 }
rkk 15:dc5753a5b83e 238
rkk 15:dc5753a5b83e 239 //saturate function
rkk 15:dc5753a5b83e 240 float MainController::saturate(float input)
rkk 15:dc5753a5b83e 241 {
rkk 15:dc5753a5b83e 242 if (input > 1.0)
rkk 15:dc5753a5b83e 243 return (1.0);
rkk 15:dc5753a5b83e 244 else if (input < -1.0)
rkk 15:dc5753a5b83e 245 return (-1.0);
rkk 15:dc5753a5b83e 246 else
rkk 15:dc5753a5b83e 247 return input;
rkk 20:6ae16da1492a 248 }