Diff: MainController.cpp

Revision:

25:4f2f441eceec

Parent:

24:9d75ed1462d6

--- a/MainController.cpp	Fri Jun 06 19:48:01 2014 +0000
+++ b/MainController.cpp	Fri Jul 11 14:30:36 2014 +0000
@@ -7,13 +7,15 @@
      ch4(p30,0.047,0.081), // adj
      ch6(p16,0.047,0.081), // frequency
      ch5(p29,0.047,0.081), // manual vs auto control
-     syren(p9,p10), //syren is 38400bps serial
+     syren(p9,p10), //syren
      //ap(p25, p26)//,
      leftservo(p21),
      rightservo(p24),
      pcam(p11,p12,p13,10),
-     //controlThread(&MainController::controlThreadStarter, this, osPriorityNormal, 1024),
-     dutyThread(&MainController::dutyThreadStarter, this, osPriorityNormal, 1024)
+     controlThread(&MainController::controlThreadStarter, this, osPriorityNormal, 1024),
+     trackerThread(&MainController::trackerThreadStarter, this, osPriorityNormal, 1024),
+     panLoop(300,100), //pgain, dgain
+     tiltLoop(500,900) //pgain, dgain
      //controlTimer(&MainController::control, osTimerPeriodic, (void*) 0)
 {
     wait_ms(50);
@@ -36,7 +38,7 @@
     raiser = 0.0;
     pitAvg = 0.0;
     alPi = 0.2/(0.2+0.001);//tf/(tf+Ts);
-    syren.baud(38400);
+    syren.baud(9600);
     dutyCycle=127;
     //pcam=new pixySPI(p11,p12,p13,10);
 
@@ -47,24 +49,30 @@
 
 MainController::~MainController()
 {
-    //delete pcam;
 }
 
-
+/*
 void MainController::dutyThreadStarter(void const* p)
 {
     MainController *instance = (MainController*)p;
     instance->updateDutyCycle();
 }
+*/
 
-/*
+void MainController::controlThreadStarter(void const* p)
+{
+    MainController *instance = (MainController*)p;
+    instance->control();
+}
+
+
 void MainController::trackerThreadStarter(void const *p)
 {
     MainController *instance = (MainController*)p;
     instance->trackTarget();
 }
-*/
 
+/*
 void MainController::updateDutyCycle()
 {
     dutyThread.signal_wait(START_THREAD);
@@ -74,132 +82,131 @@
         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
-    bool autonomous=getOpMode();
+    controlThread.signal_wait(START_THREAD);
+    while (1) {
+        //
+        //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 (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());
-        }
+        // 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 {
+                trackMutex.lock();
+                int error=CENTER_X-pcam.getBestX(); //calculate yaw difference
+                trackMutex.unlock();
+                panLoop.update(error);
+                yaw=(float(panLoop.m_pos)/1000.0f)*2-1.00f; //scale from (0,1000) -> (-1,1)
+            }
 
 
-        // 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
+            // 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;
+                if(yaw < 0.0) {
+                    ampCmd = (1.0+ yawAdjVal * yaw)*amp;
+                } else {
+                    ampCmd = amp;
+                }
+
+                fullCycle = false;
+
             } else {
-                ampCmd = amp;
-            }
-
-            fullCycle = false;
-
-        } else {
-            // reverse for the downward slope
-            amp = -amp;
+                // reverse for the downward slope
+                amp = -amp;
 
 
 
-            if(yaw > 0.0) {
-                ampCmd = (1.0- yawAdjVal *yaw)*amp;
-            } else {
-                ampCmd = 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;
             }
 
-            // use amp and frq from last cycle in order to make sure it is equalized
-            fullCycle = true;
-        }
+            // update the frequency that actually needs to be commanded
+            frqCmd = frq;
 
-        // 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
+            // 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 (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);
-    }
+        //Set Servo Values
+        if (autonomous==false)
+            pitch = this->calculatePitch();
+        else {
+            trackMutex.lock();
+            int error=pcam.getBestY()-CENTER_Y;
+            trackMutex.unlock();
+            tiltLoop.update(error);
+            pitch=float(tiltLoop.m_pos)/1000.0f;
+            //printf("Y error: %d, pitch: %f\n", error, 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;
+        }
 
 
-    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);
+        //dutyMutex.lock();
+        dutyCycle = ampCmd * saturate(raiser * sin( 2.0 * MATH_PI * frqCmd * curTime )); // add factor 4.0 to get a cut off sinus
 
-    //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;
-    dutyMutex.unlock();
-
-    //mcon.pulsewidth_ms(dutyCycle);
-    //dutyCycle=100;
-    //syren.putc(int(dutyCycle));
-    //syren.putc(int(0));
-    //trackMutex.unlock();
-    //Thread::wait(1);
-    //wait_ms(1);
-    // }
+        dutyCycle=(255/2)*dutyCycle+127;
+        if (dutyCycle<0)
+            dutyCycle=0;
+        else if (dutyCycle>255)
+            dutyCycle=255;
+        syren.putc(uint16_t(dutyCycle));
+        //Thread::wait(1);
+        //dutyMutex.unlock();
+    }
 }
 
 void MainController::trackTarget()
 {
-    if (getOpMode()==true)
-        pcam.capture();
+    trackerThread.signal_wait(START_THREAD);
+    while (1) {
+        if (getOpMode()==true) {
+            trackMutex.lock();
+            pcam.capture();
+            trackMutex.unlock();
+        }
+        Thread::wait(100);
+    }
 }
 
 float MainController::calculateFrequency()
@@ -233,12 +240,19 @@
     timer1.start();
 
     printf("start ticker\n");
-    ticker1.attach(this, &MainController::control,0.001);
-    dutyThread.signal_set(START_THREAD);
-    //trackerThread.signal_set(START_THREAD);
+    //ticker1.attach(this, &MainController::control,0.001);
+    /*
+    while (1)
+    {
+        syren.putc(int(dutyCycle));
+    }
+    */
+    controlThread.signal_set(START_THREAD);
+    //dutyThread.signal_set(START_THREAD);
+    trackerThread.signal_set(START_THREAD);
     //controlTimer.start(2);
     //control=Thread(&MainController::control);
-     tracker.attach(this, &MainController::trackTarget, 0.1); //update target position
+    //tracker.attach(this, &MainController::trackTarget, 0.1); //update target position
     //Autopilot guardian
     //ap.calibrate();
     //ap.set2D();