Revision 22:807d5467fbf6, committed 2014-06-05

Comitter:

sandwich

Date:

Thu Jun 05 00:13:02 2014 +0000

Parent:

21:835fd919b4bd

Child:

23:ef1be367726e

Commit message:

look at the previous one

Changed in this revision

--- a/MainController.cpp	Tue Jun 03 15:30:09 2014 +0000
+++ b/MainController.cpp	Thu Jun 05 00:13:02 2014 +0000
@@ -1,16 +1,19 @@
 #include "MainController.h"
 
 MainController::MainController()
-    :ch1(p18,0.056,0.090), // yaw
-     ch2(p17,0.054,0.092), // pitch
-     ch3(p15,0.054,0.092), // amplitude
-     ch4(p30,0.055,0.092), // adj
-     ch6(p16,0.053,0.092), // frequency
-     mcon(p22, p6, p7), // change pin p5 to p7, because p5 is burned through
+    :ch1(p18,0.047,0.081), // yaw
+     ch2(p17,0.047,0.081), // pitch
+     ch3(p15,0.047,0.081), // amplitude
+     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
      //ap(p25, p26)//,
      leftservo(p21),
-     rightservo(p23)
-     
+     rightservo(p23),
+     pcam(p11,p12,p13,10),
+     controlThread(&MainController::controlThreadStarter, this, osPriorityRealtime, 1024),
+     trackerThread(&MainController::trackerThreadStarter, this, osPriorityHigh, 1024)
 {
     wait_ms(50);
     amp = 0.0;
@@ -30,118 +33,177 @@
     raiser = 0.0;
     pitAvg = 0.0;
     alPi = 0.2/(0.2+0.001);//tf/(tf+Ts);
-    
-    //leftservo.calibrate(0.0008, 45); 
-    //rightservo.calibrate(-0.0001, 45); 
+    syren.baud(38400);
+    //pcam=new pixySPI(p11,p12,p13,10);
+
+    //leftservo.calibrate(0.0008, 45);
+    //rightservo.calibrate(-0.0001, 45);
+
+}
 
+MainController::~MainController()
+{
+    //delete pcam;
+}
+
+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::control()
 {
-    curTime = timer1.read();
-    
-    // check every half cycle
-    if(curTime > 1/(2*frqCmd) ) {
-        
-        // read new yaw value every half cycle
-        yaw = this->calculateYaw(); // a value from -1 to 1
-        
-        if(yaw < 0.1 && yaw > -0.1){
-            yaw =0.0;
-        }
-        // calculate liquid volume change in the chambers
-        volChg = volMax * yaw;
-        volChg = 0.0;
-        
-        timeAdd = 0.0;
-        
-        // 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
+    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
+
+        // 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
+            else {
+                //do proportional control on fish yaw
+                float gain=1/float(CENTER_X);
+                float error=pcam.getBestX()-CENTER_X; //calculate yaw difference
+                yaw=error*gain;
+                //printf("xlocation: %d -> %f\n", pcam.getBestX(), yaw);
+            }
+
+            if(yaw < 0.1 && yaw > -0.1) {
+                yaw =0.0;
+            }
+            // calculate liquid volume change in the chambers
+            volChg = volMax * yaw;
+            volChg = 0.0;
+
+            timeAdd = 0.0;
 
-            if(volChg > 0.0) {
-                // adjust frequency to add additional volume
-                if( amp < 0.0001 ) {
-                    amp = 0.0001;  
-                } 
-                timeAdd = volChg/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
 
-                if( timeAdd > 0.5/frq ) {
-                    timeAdd = 0.5/frq;
-                    volChg = timeAdd * amp;
+                if(volChg > 0.0) {
+                    // adjust frequency to add additional volume
+                    if( amp < 0.0001 ) {
+                        amp = 0.0001;
+                    }
+                    timeAdd = volChg/amp;
+
+                    if( timeAdd > 0.5/frq ) {
+                        timeAdd = 0.5/frq;
+                        volChg = timeAdd * amp;
+                    }
                 }
-            }
-            ampNew = amp;
-            
-            if(yaw < 0.0)
-            {
-                ampNew = (1.0+0.7*yaw)*amp;
-            }
-  
-            fullCycle = false;
+                ampNew = amp;
+
+                if(yaw < 0.0) {
+                    ampNew = (1.0+0.7*yaw)*amp;
+                }
+
+                fullCycle = false;
 
-        } else {
-            // reverse for the downward slope
-            amp = -amp;
+            } else {
+                // reverse for the downward slope
+                amp = -amp;
+
+                if(volChg < 0.0) {
+                    // adjust frequency to add additional volume
+                    if( amp > -0.0001 ) {
+                        amp = -0.0001;
+                    }
+                    timeAdd = volChg/amp;
 
-            if(volChg < 0.0) {
-                // adjust frequency to add additional volume
-                if( amp > -0.0001 ) {
-                    amp = -0.0001;
+                    if( timeAdd > 0.5/frq ) {
+                        timeAdd = 0.5/frq;
+                        volChg = timeAdd * amp;
+                    }
                 }
-                timeAdd = volChg/amp;
-                
-                if( timeAdd > 0.5/frq ) {
-                    timeAdd = 0.5/frq;
-                    volChg = timeAdd * amp;
+
+                ampNew = amp;
+
+                if(yaw > 0.0) {
+                    ampNew = (1.0-0.7*yaw)*amp;
                 }
+
+                // use amp and frq from last cycle in order to make sure it is equalized
+                fullCycle = true;
             }
-            
-            ampNew = amp;
-            
-            if(yaw > 0.0)
-            {
-                ampNew = (1.0-0.7*yaw)*amp;
-            }
-            
-            // 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 = 1.0/( 2.0*( timeAdd + 1/( 2* frq) ) );
+
+            // read new yaw value every half cycle
+            adj = this->calculateAdj(); // a value from 0 to 1
+
+            // for keeping track, calculate current volume storage, positive means on side is fuller, negative means other side is fuller
+            //volume = volChg + volume;
+            // rudder value used to define the trapezoid shape
+            raiser = ( 5 * adj + 1.0); // varied from 1 to 5
+
+            //reset timer
+            timer1.reset();
+            curTime = 0.0;
         }
-        // update the frequency that actually needs to be commanded
-        frqCmd = 1.0/( 2.0*( timeAdd + 1/( 2* frq) ) );
+
 
-        // read new yaw value every half cycle
-        adj = this->calculateAdj(); // a value from 0 to 1
-    
-        // for keeping track, calculate current volume storage, positive means on side is fuller, negative means other side is fuller
-        //volume = volChg + volume;            
-        // rudder value used to define the trapezoid shape
-        raiser = ( 5 * adj + 1.0); // varied from 1 to 5      
-        
-        //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/float(CENTER_Y); //try this out for now
+            float error=pcam.getBestY()-CENTER_Y;
+            pitch=error*gain;
+            //printf("ylocation: %d -> %f\n", pcam.getBestY(), pitch);
+        }
+        leftservo = pitch+0.03;
+        rightservo = 1.0 - pitch;
+        if (rightservo<0.03) {
+            rightservo = 0.03;
+        }
+
+        dutyCycle = ampNew * 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
+        dutyCycle=(255/2)*dutyCycle+127;
+        if (dutyCycle<0)
+            dutyCycle=0.0;
+        else if (dutyCycle>255)
+            dutyCycle=255.0;
+
+        //mcon.setpolarspeed(dutyCycle);
+        syren.putc(int(dutyCycle));
+        //syren.putc(int(0));
+        trackMutex.unlock();
+        Thread::wait(1);
     }
-        
-      
-    //Set Servo Values
-    pitch = this->calculatePitch();
-    leftservo = pitch+0.03;
-    rightservo = 1.0 - pitch;
-    if (rightservo<0.03){
-        rightservo = 0.03;
+}
+
+void MainController::trackTarget()
+{
+    trackerThread.signal_wait(START_THREAD);
+    while (1) {
+        //trackMutex.lock();
+        //printf("tracking\n");
+        pcam.capture();
+        //trackMutex.unlock();
     }
-        
-    dutyCycle = ampNew * saturate(raiser * sin( 2.0 * MATH_PI * frqCmd * curTime )); // add factor 4.0 to get a cut off sinus
-            
-    mcon.setpolarspeed(dutyCycle);
 }
 
 float MainController::calculateFrequency()
 {
-    return ((frqMax - frqMin) * ch6.dutycyclescaledup() + frqMin);    
+    return ((frqMax - frqMin) * ch6.dutycyclescaledup() + frqMin);
 }
 
 float MainController::calculateAmplitude()
@@ -151,7 +213,7 @@
 
 float MainController::calculateYaw()
 {
-    return (2.0*ch1.dutycyclescaledup() - 1.0); 
+    return (2.0*ch1.dutycyclescaledup() - 1.0);
 }
 
 float MainController::calculatePitch()
@@ -168,20 +230,27 @@
 void MainController::start()
 {
     timer1.start();
-    
-    ticker1.attach(this, &MainController::control,0.001);
+
+    printf("start thread\n");
+    controlThread.signal_set(START_THREAD);
+    trackerThread.signal_set(START_THREAD);
+    //ticker1.attach(this, &MainController::control,0.001);
+    //control=Thread(&MainController::control);
+    //tracker.attach(this, &MainController::trackTarget, 2.00); //update target position every second
     //Autopilot guardian
     //ap.calibrate();
     //ap.set2D();
     //ap.setoff();
-    
+
 }
 
 void MainController::stop()
 {
     timer1.stop();
-    ticker1.detach();
-    mcon.setpolarspeed(0.0);
+    //ticker1.detach();
+    //tracker.detach();
+    //mcon.setpolarspeed(0.0);
+    syren.putc(int(127));
 }
 
 float MainController::getDutyCycle()
@@ -225,6 +294,11 @@
     return timeAdd;
 }
 
+bool MainController::getOpMode()
+{
+    return ch5.dutycyclescaledup()>0.5;
+}
+
 //signum function
 float MainController::signum(float input)
 {

--- a/MainController.h	Tue Jun 03 15:30:09 2014 +0000
+++ b/MainController.h	Thu Jun 05 00:13:02 2014 +0000
@@ -2,15 +2,18 @@
 #define MBED_MAINCONTROLLER_H
  
 #include "mbed.h"
+#include "rtos.h"
 #include "PwmIn.h"
 #include "motor_controller.h"
 //#include "guardian.h"
 //#include "IMU.h"
 #include "Servo.h"
+#include "pixy.h"
 
 
 
 #define MATH_PI 3.14159265359
+#define START_THREAD 1
  
 /** MainController class to get control inputs and place them onto the system
  * 
@@ -23,6 +26,7 @@
      * @param 
      */ 
     MainController() ;
+    ~MainController();
     
     /** Start the main controller
      *
@@ -38,6 +42,7 @@
     float getPitch();
     float getTimeAdd();
     float getAdj();
+    bool getOpMode(); //returns true when in autonomous mode
 
     /** Stop the main controller
      *
@@ -48,6 +53,7 @@
  
 protected:        
     void control();
+    void trackTarget();
     float calculateFrequency();
     float calculateAmplitude();
     float calculateYaw();
@@ -62,13 +68,23 @@
     PwmIn ch3;
     PwmIn ch4;
     PwmIn ch6;
-    PololuMController mcon;
+    PwmIn ch5;
+    //PololuMController mcon;
+    Serial syren; //syren replaced the pololu controller
     //Guardian ap;
     Servo leftservo;
     Servo rightservo;
     
+    pixySPI pcam;
+    
     Timer timer1;
-    Ticker ticker1;
+    //Ticker ticker1;
+    //Ticker tracker;
+    Mutex trackMutex;
+    Thread controlThread;
+    static void controlThreadStarter(void const *p);
+    Thread trackerThread;
+    static void trackerThreadStarter(void const *p);
     float amp;
     float ampNew;
     float frq;

--- a/PwmIn.cpp	Tue Jun 03 15:30:09 2014 +0000
+++ b/PwmIn.cpp	Thu Jun 05 00:13:02 2014 +0000
@@ -46,7 +46,7 @@
 void PwmIn::fall()
 {
     _tmp = _t.read_us();
-    if(_tmp > 1000 && _tmp < 1950 && _risen == true) {
+    if(_tmp > 500 && _tmp < 5000 && _risen == true) {
         _pulsewidth = _tmp;
         _risen = false;
     }

--- a/main.cpp	Tue Jun 03 15:30:09 2014 +0000
+++ b/main.cpp	Thu Jun 05 00:13:02 2014 +0000
@@ -4,26 +4,21 @@
 //#include "PwmReader.h"
 //#include "PwmIn.h"
 #include "MainController.h"
-
 Serial pc(USBTX, USBRX);
 Timer t;
 
 int main()
 {
     t.start();
-    
+
     MainController mainCtrl;
-    
-    // pixy cam!
-    pixySPI pcam(p5,p6,p7, 20, &pc);
-    //
-       
+
     mainCtrl.start();
 
+    //printf("started\n");
     while(true) {
-  
-        pc.printf("frq: %.2f, vol: %.2f, amp: %.2f, yaw: %.2f, dut: %.2f, pit: %.2f, adj: %.2f, tadd: %.2f, t: %.2f\n",
-         mainCtrl.getFrequency(), mainCtrl.getVolume(), mainCtrl.getAmplitude(), mainCtrl.getYaw(), mainCtrl.getDutyCycle(), mainCtrl.getPitch(), mainCtrl.getAdj(), mainCtrl.getTimeAdd(), t.read());
+        pc.printf("control: %d, frq: %.3f, vol: %.2f, amp: %.3f, yaw: %.2f, dut: %.2f, pit: %.2f, adj: %.2f, tadd: %.2f, t: %.2f\n",
+                  mainCtrl.getOpMode(), mainCtrl.getFrequency(), mainCtrl.getVolume(), mainCtrl.getAmplitude(), mainCtrl.getYaw(), mainCtrl.getDutyCycle(), mainCtrl.getPitch(), mainCtrl.getAdj(), mainCtrl.getTimeAdd(), t.read());
 
         wait_ms(100);
     }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Thu Jun 05 00:13:02 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#015df9e602b6

--- a/pixy_cam.lib	Tue Jun 03 15:30:09 2014 +0000
+++ b/pixy_cam.lib	Thu Jun 05 00:13:02 2014 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/teams/jetfishteam/code/pixy_cam/#b3a6b5a898b4
+http://mbed.org/teams/jetfishteam/code/pixy_cam/#f54759b26096