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@22:807d5467fbf6, 2014-06-05 (annotated)

Committer:
sandwich
Date:
Thu Jun 05 00:13:02 2014 +0000
Revision:
22:807d5467fbf6
Parent:
20:6ae16da1492a
Child:
23:ef1be367726e
look at the previous one

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()
sandwich 22:807d5467fbf6 4 :ch1(p18,0.047,0.081), // yaw
sandwich 22:807d5467fbf6 5 ch2(p17,0.047,0.081), // pitch
sandwich 22:807d5467fbf6 6 ch3(p15,0.047,0.081), // amplitude
sandwich 22:807d5467fbf6 7 ch4(p30,0.047,0.081), // adj
sandwich 22:807d5467fbf6 8 ch6(p16,0.047,0.081), // frequency
sandwich 22:807d5467fbf6 9 ch5(p29,0.047,0.081), // manual vs auto control
sandwich 22:807d5467fbf6 10 syren(p9,p10), //syren is 38400bps serial
rkk 20:6ae16da1492a 11 //ap(p25, p26)//,
rkk 20:6ae16da1492a 12 leftservo(p21),
sandwich 22:807d5467fbf6 13 rightservo(p23),
sandwich 22:807d5467fbf6 14 pcam(p11,p12,p13,10),
sandwich 22:807d5467fbf6 15 controlThread(&MainController::controlThreadStarter, this, osPriorityRealtime, 1024),
sandwich 22:807d5467fbf6 16 trackerThread(&MainController::trackerThreadStarter, this, osPriorityHigh, 1024)
rkk 11:8ec915eb70f6 17 {
rkk 12:7eeb29892625 18 wait_ms(50);
rkk 19:655db88b045c 19 amp = 0.0;
rkk 17:6970aef8154b 20 frq = 1.0;
rkk 19:655db88b045c 21 frqCmd = frq;
rkk 19:655db88b045c 22 yaw = 0.5;
rkk 17:6970aef8154b 23 pitch = 0.5;
rkk 20:6ae16da1492a 24 adj = 0.5;
rkk 20:6ae16da1492a 25 frqMin = 0.8; //hardcoded
rkk 20:6ae16da1492a 26 frqMax = 2.9; //hardcoded
rkk 19:655db88b045c 27 //change = 0;
rkk 19:655db88b045c 28 //state = 0;
rkk 19:655db88b045c 29 fullCycle = true;
rkk 19:655db88b045c 30 volume = 0.0;
rkk 19:655db88b045c 31 volMax = 0.1;
rkk 19:655db88b045c 32 volChg = 0.0;
rkk 20:6ae16da1492a 33 raiser = 0.0;
rkk 20:6ae16da1492a 34 pitAvg = 0.0;
rkk 20:6ae16da1492a 35 alPi = 0.2/(0.2+0.001);//tf/(tf+Ts);
sandwich 22:807d5467fbf6 36 syren.baud(38400);
sandwich 22:807d5467fbf6 37 //pcam=new pixySPI(p11,p12,p13,10);
sandwich 22:807d5467fbf6 38
sandwich 22:807d5467fbf6 39 //leftservo.calibrate(0.0008, 45);
sandwich 22:807d5467fbf6 40 //rightservo.calibrate(-0.0001, 45);
sandwich 22:807d5467fbf6 41
sandwich 22:807d5467fbf6 42 }
rkk 20:6ae16da1492a 43
sandwich 22:807d5467fbf6 44 MainController::~MainController()
sandwich 22:807d5467fbf6 45 {
sandwich 22:807d5467fbf6 46 //delete pcam;
sandwich 22:807d5467fbf6 47 }
sandwich 22:807d5467fbf6 48
sandwich 22:807d5467fbf6 49 void MainController::controlThreadStarter(void const *p)
sandwich 22:807d5467fbf6 50 {
sandwich 22:807d5467fbf6 51 MainController *instance = (MainController*)p;
sandwich 22:807d5467fbf6 52 instance->control();
sandwich 22:807d5467fbf6 53 }
sandwich 22:807d5467fbf6 54
sandwich 22:807d5467fbf6 55 void MainController::trackerThreadStarter(void const *p)
sandwich 22:807d5467fbf6 56 {
sandwich 22:807d5467fbf6 57 MainController *instance = (MainController*)p;
sandwich 22:807d5467fbf6 58 instance->trackTarget();
rkk 11:8ec915eb70f6 59 }
rkk 11:8ec915eb70f6 60
rkk 11:8ec915eb70f6 61 void MainController::control()
rkk 11:8ec915eb70f6 62 {
sandwich 22:807d5467fbf6 63 controlThread.signal_wait(START_THREAD);
sandwich 22:807d5467fbf6 64 while (1) {
sandwich 22:807d5467fbf6 65 trackMutex.lock();
sandwich 22:807d5467fbf6 66 curTime = timer1.read();
sandwich 22:807d5467fbf6 67 //Block bestObject=pcam.getBlocks()[0]; //our best bet is the first block because it has the biggest area
sandwich 22:807d5467fbf6 68
sandwich 22:807d5467fbf6 69 // check every half cycle
sandwich 22:807d5467fbf6 70 if(curTime > 1/(2*frqCmd) ) {
sandwich 22:807d5467fbf6 71 // read new yaw value every half cycle
sandwich 22:807d5467fbf6 72 if (ch5.dutycyclescaledup()<=0.5)
sandwich 22:807d5467fbf6 73 yaw = this->calculateYaw(); // a value from -1 to 1
sandwich 22:807d5467fbf6 74 else {
sandwich 22:807d5467fbf6 75 //do proportional control on fish yaw
sandwich 22:807d5467fbf6 76 float gain=1/float(CENTER_X);
sandwich 22:807d5467fbf6 77 float error=pcam.getBestX()-CENTER_X; //calculate yaw difference
sandwich 22:807d5467fbf6 78 yaw=error*gain;
sandwich 22:807d5467fbf6 79 //printf("xlocation: %d -> %f\n", pcam.getBestX(), yaw);
sandwich 22:807d5467fbf6 80 }
sandwich 22:807d5467fbf6 81
sandwich 22:807d5467fbf6 82 if(yaw < 0.1 && yaw > -0.1) {
sandwich 22:807d5467fbf6 83 yaw =0.0;
sandwich 22:807d5467fbf6 84 }
sandwich 22:807d5467fbf6 85 // calculate liquid volume change in the chambers
sandwich 22:807d5467fbf6 86 volChg = volMax * yaw;
sandwich 22:807d5467fbf6 87 volChg = 0.0;
sandwich 22:807d5467fbf6 88
sandwich 22:807d5467fbf6 89 timeAdd = 0.0;
rkk 19:655db88b045c 90
sandwich 22:807d5467fbf6 91 // Read volume and frequency only every full cycle
sandwich 22:807d5467fbf6 92 if( fullCycle ) {
sandwich 22:807d5467fbf6 93 //read other new inputs
sandwich 22:807d5467fbf6 94 amp = this->calculateAmplitude(); // a value from 0 to 1
sandwich 22:807d5467fbf6 95 frq = this->calculateFrequency(); // a value from frqmin to frqmax
rkk 19:655db88b045c 96
sandwich 22:807d5467fbf6 97 if(volChg > 0.0) {
sandwich 22:807d5467fbf6 98 // adjust frequency to add additional volume
sandwich 22:807d5467fbf6 99 if( amp < 0.0001 ) {
sandwich 22:807d5467fbf6 100 amp = 0.0001;
sandwich 22:807d5467fbf6 101 }
sandwich 22:807d5467fbf6 102 timeAdd = volChg/amp;
sandwich 22:807d5467fbf6 103
sandwich 22:807d5467fbf6 104 if( timeAdd > 0.5/frq ) {
sandwich 22:807d5467fbf6 105 timeAdd = 0.5/frq;
sandwich 22:807d5467fbf6 106 volChg = timeAdd * amp;
sandwich 22:807d5467fbf6 107 }
rkk 19:655db88b045c 108 }
sandwich 22:807d5467fbf6 109 ampNew = amp;
sandwich 22:807d5467fbf6 110
sandwich 22:807d5467fbf6 111 if(yaw < 0.0) {
sandwich 22:807d5467fbf6 112 ampNew = (1.0+0.7*yaw)*amp;
sandwich 22:807d5467fbf6 113 }
sandwich 22:807d5467fbf6 114
sandwich 22:807d5467fbf6 115 fullCycle = false;
rkk 19:655db88b045c 116
sandwich 22:807d5467fbf6 117 } else {
sandwich 22:807d5467fbf6 118 // reverse for the downward slope
sandwich 22:807d5467fbf6 119 amp = -amp;
sandwich 22:807d5467fbf6 120
sandwich 22:807d5467fbf6 121 if(volChg < 0.0) {
sandwich 22:807d5467fbf6 122 // adjust frequency to add additional volume
sandwich 22:807d5467fbf6 123 if( amp > -0.0001 ) {
sandwich 22:807d5467fbf6 124 amp = -0.0001;
sandwich 22:807d5467fbf6 125 }
sandwich 22:807d5467fbf6 126 timeAdd = volChg/amp;
rkk 19:655db88b045c 127
sandwich 22:807d5467fbf6 128 if( timeAdd > 0.5/frq ) {
sandwich 22:807d5467fbf6 129 timeAdd = 0.5/frq;
sandwich 22:807d5467fbf6 130 volChg = timeAdd * amp;
sandwich 22:807d5467fbf6 131 }
rkk 19:655db88b045c 132 }
sandwich 22:807d5467fbf6 133
sandwich 22:807d5467fbf6 134 ampNew = amp;
sandwich 22:807d5467fbf6 135
sandwich 22:807d5467fbf6 136 if(yaw > 0.0) {
sandwich 22:807d5467fbf6 137 ampNew = (1.0-0.7*yaw)*amp;
rkk 19:655db88b045c 138 }
sandwich 22:807d5467fbf6 139
sandwich 22:807d5467fbf6 140 // use amp and frq from last cycle in order to make sure it is equalized
sandwich 22:807d5467fbf6 141 fullCycle = true;
rkk 19:655db88b045c 142 }
sandwich 22:807d5467fbf6 143 // update the frequency that actually needs to be commanded
sandwich 22:807d5467fbf6 144 frqCmd = 1.0/( 2.0*( timeAdd + 1/( 2* frq) ) );
sandwich 22:807d5467fbf6 145
sandwich 22:807d5467fbf6 146 // read new yaw value every half cycle
sandwich 22:807d5467fbf6 147 adj = this->calculateAdj(); // a value from 0 to 1
sandwich 22:807d5467fbf6 148
sandwich 22:807d5467fbf6 149 // for keeping track, calculate current volume storage, positive means on side is fuller, negative means other side is fuller
sandwich 22:807d5467fbf6 150 //volume = volChg + volume;
sandwich 22:807d5467fbf6 151 // rudder value used to define the trapezoid shape
sandwich 22:807d5467fbf6 152 raiser = ( 5 * adj + 1.0); // varied from 1 to 5
sandwich 22:807d5467fbf6 153
sandwich 22:807d5467fbf6 154 //reset timer
sandwich 22:807d5467fbf6 155 timer1.reset();
sandwich 22:807d5467fbf6 156 curTime = 0.0;
rkk 19:655db88b045c 157 }
sandwich 22:807d5467fbf6 158
rkk 19:655db88b045c 159
sandwich 22:807d5467fbf6 160 //Set Servo Values
sandwich 22:807d5467fbf6 161 if (ch5.dutycyclescaledup()<=0.5)
sandwich 22:807d5467fbf6 162 pitch = this->calculatePitch();
sandwich 22:807d5467fbf6 163 else {
sandwich 22:807d5467fbf6 164 //do a proportional control on the error since servos already give you position control
sandwich 22:807d5467fbf6 165 float gain=1/float(CENTER_Y); //try this out for now
sandwich 22:807d5467fbf6 166 float error=pcam.getBestY()-CENTER_Y;
sandwich 22:807d5467fbf6 167 pitch=error*gain;
sandwich 22:807d5467fbf6 168 //printf("ylocation: %d -> %f\n", pcam.getBestY(), pitch);
sandwich 22:807d5467fbf6 169 }
sandwich 22:807d5467fbf6 170 leftservo = pitch+0.03;
sandwich 22:807d5467fbf6 171 rightservo = 1.0 - pitch;
sandwich 22:807d5467fbf6 172 if (rightservo<0.03) {
sandwich 22:807d5467fbf6 173 rightservo = 0.03;
sandwich 22:807d5467fbf6 174 }
sandwich 22:807d5467fbf6 175
sandwich 22:807d5467fbf6 176 dutyCycle = ampNew * saturate(raiser * sin( 2.0 * MATH_PI * frqCmd * curTime )); // add factor 4.0 to get a cut off sinus
sandwich 22:807d5467fbf6 177
sandwich 22:807d5467fbf6 178 //now scale duty cycle from -1 to 1 -> 0 to 255
sandwich 22:807d5467fbf6 179 dutyCycle=(255/2)*dutyCycle+127;
sandwich 22:807d5467fbf6 180 if (dutyCycle<0)
sandwich 22:807d5467fbf6 181 dutyCycle=0.0;
sandwich 22:807d5467fbf6 182 else if (dutyCycle>255)
sandwich 22:807d5467fbf6 183 dutyCycle=255.0;
sandwich 22:807d5467fbf6 184
sandwich 22:807d5467fbf6 185 //mcon.setpolarspeed(dutyCycle);
sandwich 22:807d5467fbf6 186 syren.putc(int(dutyCycle));
sandwich 22:807d5467fbf6 187 //syren.putc(int(0));
sandwich 22:807d5467fbf6 188 trackMutex.unlock();
sandwich 22:807d5467fbf6 189 Thread::wait(1);
rkk 19:655db88b045c 190 }
sandwich 22:807d5467fbf6 191 }
sandwich 22:807d5467fbf6 192
sandwich 22:807d5467fbf6 193 void MainController::trackTarget()
sandwich 22:807d5467fbf6 194 {
sandwich 22:807d5467fbf6 195 trackerThread.signal_wait(START_THREAD);
sandwich 22:807d5467fbf6 196 while (1) {
sandwich 22:807d5467fbf6 197 //trackMutex.lock();
sandwich 22:807d5467fbf6 198 //printf("tracking\n");
sandwich 22:807d5467fbf6 199 pcam.capture();
sandwich 22:807d5467fbf6 200 //trackMutex.unlock();
rkk 20:6ae16da1492a 201 }
rkk 11:8ec915eb70f6 202 }
rkk 12:7eeb29892625 203
rkk 11:8ec915eb70f6 204 float MainController::calculateFrequency()
rkk 11:8ec915eb70f6 205 {
sandwich 22:807d5467fbf6 206 return ((frqMax - frqMin) * ch6.dutycyclescaledup() + frqMin);
rkk 11:8ec915eb70f6 207 }
rkk 11:8ec915eb70f6 208
rkk 19:655db88b045c 209 float MainController::calculateAmplitude()
rkk 11:8ec915eb70f6 210 {
rkk 20:6ae16da1492a 211 return (ch3.dutycyclescaledup());
rkk 11:8ec915eb70f6 212 }
rkk 11:8ec915eb70f6 213
rkk 19:655db88b045c 214 float MainController::calculateYaw()
rkk 12:7eeb29892625 215 {
sandwich 22:807d5467fbf6 216 return (2.0*ch1.dutycyclescaledup() - 1.0);
rkk 12:7eeb29892625 217 }
rkk 12:7eeb29892625 218
rkk 17:6970aef8154b 219 float MainController::calculatePitch()
rkk 17:6970aef8154b 220 {
rkk 20:6ae16da1492a 221 pitAvg = alPi * pitAvg+ (1.0 - alPi)*(ch2.dutycyclescaledup());
rkk 20:6ae16da1492a 222 return pitAvg;
rkk 20:6ae16da1492a 223 }
rkk 20:6ae16da1492a 224
rkk 20:6ae16da1492a 225 float MainController::calculateAdj()
rkk 20:6ae16da1492a 226 {
rkk 20:6ae16da1492a 227 return (ch4.dutycyclescaledup());
rkk 17:6970aef8154b 228 }
rkk 17:6970aef8154b 229
rkk 11:8ec915eb70f6 230 void MainController::start()
rkk 11:8ec915eb70f6 231 {
rkk 11:8ec915eb70f6 232 timer1.start();
sandwich 22:807d5467fbf6 233
sandwich 22:807d5467fbf6 234 printf("start thread\n");
sandwich 22:807d5467fbf6 235 controlThread.signal_set(START_THREAD);
sandwich 22:807d5467fbf6 236 trackerThread.signal_set(START_THREAD);
sandwich 22:807d5467fbf6 237 //ticker1.attach(this, &MainController::control,0.001);
sandwich 22:807d5467fbf6 238 //control=Thread(&MainController::control);
sandwich 22:807d5467fbf6 239 //tracker.attach(this, &MainController::trackTarget, 2.00); //update target position every second
rkk 12:7eeb29892625 240 //Autopilot guardian
rkk 12:7eeb29892625 241 //ap.calibrate();
rkk 12:7eeb29892625 242 //ap.set2D();
rkk 20:6ae16da1492a 243 //ap.setoff();
sandwich 22:807d5467fbf6 244
rkk 11:8ec915eb70f6 245 }
rkk 11:8ec915eb70f6 246
rkk 11:8ec915eb70f6 247 void MainController::stop()
rkk 11:8ec915eb70f6 248 {
rkk 11:8ec915eb70f6 249 timer1.stop();
sandwich 22:807d5467fbf6 250 //ticker1.detach();
sandwich 22:807d5467fbf6 251 //tracker.detach();
sandwich 22:807d5467fbf6 252 //mcon.setpolarspeed(0.0);
sandwich 22:807d5467fbf6 253 syren.putc(int(127));
rkk 11:8ec915eb70f6 254 }
rkk 11:8ec915eb70f6 255
rkk 11:8ec915eb70f6 256 float MainController::getDutyCycle()
rkk 11:8ec915eb70f6 257 {
rkk 11:8ec915eb70f6 258 return dutyCycle;
rkk 11:8ec915eb70f6 259 }
rkk 11:8ec915eb70f6 260
rkk 11:8ec915eb70f6 261 float MainController::getAmplitude()
rkk 11:8ec915eb70f6 262 {
rkk 19:655db88b045c 263 return amp;
rkk 11:8ec915eb70f6 264 }
rkk 11:8ec915eb70f6 265
rkk 11:8ec915eb70f6 266
rkk 11:8ec915eb70f6 267 float MainController::getFrequency()
rkk 11:8ec915eb70f6 268 {
rkk 11:8ec915eb70f6 269 return frq;
rkk 11:8ec915eb70f6 270 }
rkk 11:8ec915eb70f6 271
rkk 11:8ec915eb70f6 272 float MainController::getVolume()
rkk 11:8ec915eb70f6 273 {
rkk 19:655db88b045c 274 return volume;
rkk 11:8ec915eb70f6 275 }
rkk 12:7eeb29892625 276
rkk 19:655db88b045c 277 float MainController::getYaw()
rkk 12:7eeb29892625 278 {
rkk 19:655db88b045c 279 return yaw;
rkk 12:7eeb29892625 280 }
rkk 12:7eeb29892625 281
rkk 17:6970aef8154b 282 float MainController::getPitch()
rkk 17:6970aef8154b 283 {
rkk 17:6970aef8154b 284 return pitch;
rkk 17:6970aef8154b 285 }
rkk 17:6970aef8154b 286
rkk 20:6ae16da1492a 287 float MainController::getAdj()
rkk 20:6ae16da1492a 288 {
rkk 20:6ae16da1492a 289 return adj;
rkk 20:6ae16da1492a 290 }
rkk 20:6ae16da1492a 291
rkk 19:655db88b045c 292 float MainController::getTimeAdd()
rkk 19:655db88b045c 293 {
rkk 19:655db88b045c 294 return timeAdd;
rkk 19:655db88b045c 295 }
rkk 19:655db88b045c 296
sandwich 22:807d5467fbf6 297 bool MainController::getOpMode()
sandwich 22:807d5467fbf6 298 {
sandwich 22:807d5467fbf6 299 return ch5.dutycyclescaledup()>0.5;
sandwich 22:807d5467fbf6 300 }
sandwich 22:807d5467fbf6 301
rkk 15:dc5753a5b83e 302 //signum function
rkk 13:5ed8fd870723 303 float MainController::signum(float input)
rkk 13:5ed8fd870723 304 {
rkk 13:5ed8fd870723 305 if (input>0.0)
rkk 14:a5389e26a63b 306 return 1.0;
rkk 13:5ed8fd870723 307 else if (input<0.0)
rkk 14:a5389e26a63b 308 return (-1.0);
rkk 13:5ed8fd870723 309 else
rkk 14:a5389e26a63b 310 return 0.0;
rkk 15:dc5753a5b83e 311 }
rkk 15:dc5753a5b83e 312
rkk 15:dc5753a5b83e 313 //saturate function
rkk 15:dc5753a5b83e 314 float MainController::saturate(float input)
rkk 15:dc5753a5b83e 315 {
rkk 15:dc5753a5b83e 316 if (input > 1.0)
rkk 15:dc5753a5b83e 317 return (1.0);
rkk 15:dc5753a5b83e 318 else if (input < -1.0)
rkk 15:dc5753a5b83e 319 return (-1.0);
rkk 15:dc5753a5b83e 320 else
rkk 15:dc5753a5b83e 321 return input;
rkk 20:6ae16da1492a 322 }