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@25:4f2f441eceec, 2014-07-11 (annotated)

Committer:
sandwich
Date:
Fri Jul 11 14:30:36 2014 +0000
Revision:
25:4f2f441eceec
Parent:
24:9d75ed1462d6 
latest revision

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