Diff: MainController.cpp

Revision:

24:9d75ed1462d6

Parent:

23:ef1be367726e

Child:

25:4f2f441eceec

--- a/MainController.cpp	Thu Jun 05 17:32:51 2014 +0000
+++ b/MainController.cpp	Fri Jun 06 19:48:01 2014 +0000
@@ -2,7 +2,7 @@
 
 MainController::MainController()
     :ch1(p18,0.047,0.081), // yaw
-     ch2(p17,0.047,0.081), // pitch
+     ch2(p17,0.047,0.09), // pitch
      ch3(p15,0.047,0.081), // amplitude
      ch4(p30,0.047,0.081), // adj
      ch6(p16,0.047,0.081), // frequency
@@ -10,10 +10,11 @@
      syren(p9,p10), //syren is 38400bps serial
      //ap(p25, p26)//,
      leftservo(p21),
-     rightservo(p23),
+     rightservo(p24),
      pcam(p11,p12,p13,10),
-     controlThread(&MainController::controlThreadStarter, this, osPriorityRealtime, 1024),
-     trackerThread(&MainController::trackerThreadStarter, this, osPriorityHigh, 1024)
+     //controlThread(&MainController::controlThreadStarter, this, osPriorityNormal, 1024),
+     dutyThread(&MainController::dutyThreadStarter, this, osPriorityNormal, 1024)
+     //controlTimer(&MainController::control, osTimerPeriodic, (void*) 0)
 {
     wait_ms(50);
     amp = 0.0;
@@ -36,6 +37,7 @@
     pitAvg = 0.0;
     alPi = 0.2/(0.2+0.001);//tf/(tf+Ts);
     syren.baud(38400);
+    dutyCycle=127;
     //pcam=new pixySPI(p11,p12,p13,10);
 
     //leftservo.calibrate(0.0008, 45);
@@ -48,141 +50,156 @@
     //delete pcam;
 }
 
-void MainController::controlThreadStarter(void const *p)
+
+void MainController::dutyThreadStarter(void const* p)
 {
     MainController *instance = (MainController*)p;
-    instance->control();
+    instance->updateDutyCycle();
 }
 
+/*
 void MainController::trackerThreadStarter(void const *p)
 {
     MainController *instance = (MainController*)p;
     instance->trackTarget();
 }
+*/
 
+void MainController::updateDutyCycle()
+{
+    dutyThread.signal_wait(START_THREAD);
+    while (1) {
+        dutyMutex.lock();
+        syren.putc(int(dutyCycle));
+        dutyMutex.unlock();
+    }
+}
 void MainController::control()
 {
-    controlThread.signal_wait(START_THREAD);
-    while (1) {
-        trackMutex.lock();
-        curTime = timer1.read();
-        //Block bestObject=pcam.getBlocks()[0]; //our best bet is the first block because it has the biggest area
+    //controlThread.signal_wait(START_THREAD);
+    //while (1) {
+    //trackMutex.lock();
+    curTime = timer1.read();
+    //Block bestObject=pcam.getBlocks()[0]; //our best bet is the first block because it has the biggest area
+    bool autonomous=getOpMode();
 
-        // check every half cycle
-        if(curTime > 1/(2*frqCmd) ) {
-            // read new yaw value every half cycle
-            if (ch5.dutycyclescaledup()<=0.5) {
-                yaw = this->calculateYaw(); // a value from -1 to 1
-                if(yaw < 0.1 && yaw > -0.1)
-                    yaw =0.0;
+    // check every half cycle
+    if(curTime > 1/(2*frqCmd) ) {
+        // read new yaw value every half cycle
+        if (autonomous==false) {
+            yaw = this->calculateYaw(); // a value from -1 to 1
+            if(yaw < 0.1 && yaw > -0.1)
+                yaw =0.0;
+        } else {
+            //do proportional control on fish yaw
+            float gain=(1/(1+ampCmd))*frqCmd*(1/float(CENTER_X));
+            //trackMutex.lock();
+            float error=pcam.getBestX()-CENTER_X; //calculate yaw difference
+            //trackMutex.unlock();
+            yaw=error*gain+0.5f;
+            //printf("x: %d, ", pcam.getBestX());
+        }
+
+
+        // Read volume and frequency only every full cycle
+        if( fullCycle ) {
+            //read other new inputs
+            amp = this->calculateAmplitude(); // a value from 0 to 1
+            frq = this->calculateFrequency(); // a value from frqmin to frqmax
+
+            if(yaw < 0.0) {
+                ampCmd = (1.0+ yawAdjVal * yaw)*amp;
             } else {
-                //do proportional control on fish yaw
-                float gain=(1/(1+ampCmd))*frqCmd*(1/float(CENTER_X));
-                float error=pcam.getBestX()-CENTER_X; //calculate yaw difference
-                yaw=error*gain+0.5f;
-                //printf("x: %d, ", pcam.getBestX());
+                ampCmd = amp;
             }
 
-
-            // Read volume and frequency only every full cycle
-            if( fullCycle ) {
-                //read other new inputs
-                amp = this->calculateAmplitude(); // a value from 0 to 1
-                frq = this->calculateFrequency(); // a value from frqmin to frqmax
+            fullCycle = false;
 
-                if(yaw < 0.0) {
-                    ampCmd = (1.0+ yawAdjVal * yaw)*amp;
-                } else {
-                    ampCmd = amp;
-                }
-
-                fullCycle = false;
-
-            } else {
-                // reverse for the downward slope
-                amp = -amp;
+        } else {
+            // reverse for the downward slope
+            amp = -amp;
 
 
 
-                if(yaw > 0.0) {
-                    ampCmd = (1.0- yawAdjVal *yaw)*amp;
-                } else {
-                    ampCmd = amp;
-                }
-
-                // use amp and frq from last cycle in order to make sure it is equalized
-                fullCycle = true;
+            if(yaw > 0.0) {
+                ampCmd = (1.0- yawAdjVal *yaw)*amp;
+            } else {
+                ampCmd = amp;
             }
 
-            // update the frequency that actually needs to be commanded
-            frqCmd = frq;
+            // use amp and frq from last cycle in order to make sure it is equalized
+            fullCycle = true;
+        }
 
-            // read new yaw value every half cycle
-            adj = this->calculateAdj(); // a value from 0 to 1
+        // update the frequency that actually needs to be commanded
+        frqCmd = frq;
+
+        // read new yaw value every half cycle
+        adj = this->calculateAdj(); // a value from 0 to 1
 
 
-            // adj value used to define the trapezoid shape
-            raiser = ( 5 * 0.7 + 1.0); // varied from 1 to 5 for now hard coded
+        // adj value used to define the trapezoid shape
+        raiser = ( 5 * 0.7 + 1.0); // varied from 1 to 5 for now hard coded
 
-            //reset timer
-            timer1.reset();
-            curTime = 0.0;
-        }
+        //reset timer
+        timer1.reset();
+        curTime = 0.0;
+    }
 
 
-        //Set Servo Values
-        if (ch5.dutycyclescaledup()<=0.5)
-            pitch = this->calculatePitch();
-        else {
-            //do a proportional control on the error since servos already give you position control
-            float gain=(1/(1+abs(ampCmd)))*(1/frqCmd)*(1/float(CENTER_Y))*adj; //try this out for now
-            //float gain=(1/float(CENTER_Y))*adj;
-            float error=pcam.getBestY()-CENTER_Y;
-            pitch=error*gain+0.5;
-            //printf("error: %f\n", error);
-            //printf("y: %d\n", pcam.getBestY());
-            //printf("pitch: %f\n", pitch);
-        }
+    //Set Servo Values
+    if (autonomous==false)
+        pitch = this->calculatePitch();
+    else {
+        //do a proportional control on the error since servos already give you position control
+        float gain=(1/(1+abs(ampCmd)))*(1/frqCmd)*(1/float(CENTER_Y))*adj; //try this out for now
+        //float gain=(1/float(CENTER_Y))*adj;
+        float error=pcam.getBestY()-CENTER_Y;
+        pitch=error*gain+0.5;
+        //printf("error: %f\n", error);
+        //printf("y: %d\n", pcam.getBestY());
+        //printf("pitch: %f\n", pitch);
+    }
 
-        //Adjusting the trim of the pitch angles
-        leftservo = pitch+0.03;
-        if (leftservo > 1.0) {
-            leftservo = 1.0;
-        }
-        rightservo = 1.0 - pitch;
-        if (rightservo < 0.03) {
-            rightservo = 0.03;
-        }
+    //Adjusting the trim of the pitch angles
+    leftservo = pitch+0.03;
+    if (leftservo > 1.0) {
+        leftservo = 1.0;
+    }
+    rightservo = 1.0 - pitch;
+    if (rightservo < 0.03) {
+        rightservo = 0.03;
+    }
 
 
-        dutyCycle = ampCmd * saturate(raiser * sin( 2.0 * MATH_PI * frqCmd * curTime )); // add factor 4.0 to get a cut off sinus
-        //printf("dc: %f\n", dutyCycle);
+    dutyMutex.lock();
+    dutyCycle = ampCmd * saturate(raiser * sin( 2.0 * MATH_PI * frqCmd * curTime )); // add factor 4.0 to get a cut off sinus
+    //printf("dc: %f\n", dutyCycle);
+
+    //now scale duty cycle from -1 to 1 -> 0 to 255
+
 
-        //now scale duty cycle from -1 to 1 -> 0 to 255
-        dutyCycle=(255/2)*dutyCycle+127;
-        if (dutyCycle<0)
-            dutyCycle=0.0;
-        else if (dutyCycle>255)
-            dutyCycle=255.0;
+    dutyCycle=(255/2)*dutyCycle+127;
+    if (dutyCycle<0)
+        dutyCycle=0.0;
+    else if (dutyCycle>255)
+        dutyCycle=255.0;
+    dutyMutex.unlock();
 
-        //mcon.setpolarspeed(dutyCycle);
-        //dutyCycle=100;
-        syren.putc(int(dutyCycle));
-        //syren.putc(int(0));
-        trackMutex.unlock();
-        Thread::wait(1);
-    }
+    //mcon.pulsewidth_ms(dutyCycle);
+    //dutyCycle=100;
+    //syren.putc(int(dutyCycle));
+    //syren.putc(int(0));
+    //trackMutex.unlock();
+    //Thread::wait(1);
+    //wait_ms(1);
+    // }
 }
 
 void MainController::trackTarget()
 {
-    trackerThread.signal_wait(START_THREAD);
-    while (1) {
-        //trackMutex.lock();
-        //printf("tracking\n");
+    if (getOpMode()==true)
         pcam.capture();
-        //trackMutex.unlock();
-    }
 }
 
 float MainController::calculateFrequency()
@@ -215,16 +232,26 @@
 {
     timer1.start();
 
-    printf("start thread\n");
-    controlThread.signal_set(START_THREAD);
-    trackerThread.signal_set(START_THREAD);
-    //ticker1.attach(this, &MainController::control,0.001);
+    printf("start ticker\n");
+    ticker1.attach(this, &MainController::control,0.001);
+    dutyThread.signal_set(START_THREAD);
+    //trackerThread.signal_set(START_THREAD);
+    //controlTimer.start(2);
     //control=Thread(&MainController::control);
-    //tracker.attach(this, &MainController::trackTarget, 2.00); //update target position every second
+     tracker.attach(this, &MainController::trackTarget, 0.1); //update target position
     //Autopilot guardian
     //ap.calibrate();
     //ap.set2D();
     //ap.setoff();
+    //RtosTimer controlTimer((void*)(&control), osTimerPeriodic, (void *)0);
+    //controlTimer.start(5);
+    //while(true) {
+    //printf("control: %d, frq: %.3f, vol: %.2f, amp: %.3f, yaw: %.2f, dut: %.2f, pit: %.2f, adj: %.2f, t: %.2f\n",
+    //          mainCtrl.getOpMode(), mainCtrl.getFrequency(), mainCtrl.getVolume(), mainCtrl.getAmplitude(), mainCtrl.getYaw(), mainCtrl.getDutyCycle(), mainCtrl.getPitch(), mainCtrl.getAdj(), t.read());
+
+    //Thread::wait(100);
+    //wait_ms(100);
+
 
 }