Revision 25:4f2f441eceec, committed 2014-07-11

Comitter:

sandwich

Date:

Fri Jul 11 14:30:36 2014 +0000

Parent:

24:9d75ed1462d6

Commit message:

latest revision

Changed in this revision

diff -r 9d75ed1462d6 -r 4f2f441eceec MainController.cpp
--- 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();

diff -r 9d75ed1462d6 -r 4f2f441eceec MainController.h
--- a/MainController.h	Fri Jun 06 19:48:01 2014 +0000
+++ b/MainController.h	Fri Jul 11 14:30:36 2014 +0000
@@ -1,6 +1,6 @@
 #ifndef MBED_MAINCONTROLLER_H
 #define MBED_MAINCONTROLLER_H
- 
+
 #include "mbed.h"
 #include "rtos.h"
 #include "PwmIn.h"
@@ -9,31 +9,33 @@
 //#include "IMU.h"
 #include "Servo.h"
 #include "pixy.h"
+#include "servoloop.h"
 
 
 
 #define MATH_PI 3.14159265359
 #define START_THREAD 1
- 
+
 /** MainController class to get control inputs and place them onto the system
- * 
- * 
+ *
+ *
  */
-class MainController {
+class MainController
+{
 public:
     /** Create a MainController
      *
-     * @param 
-     */ 
+     * @param
+     */
     MainController() ;
     ~MainController();
-    
+
     /** Start the main controller
      *
-     * @returns 
+     * @returns
      */
     void start();
-    
+
     float getDutyCycle();
     float getFrequency();
     float getVolume();
@@ -45,12 +47,12 @@
 
     /** Stop the main controller
      *
-     * @returns 
+     * @returns
      */
     void stop();
-    
- 
-protected:        
+
+
+protected:
     void control();
     void trackTarget();
     float calculateFrequency();
@@ -60,7 +62,7 @@
     float calculateAdj();
     float signum(float input);
     float saturate(float input);
-    
+
 private:
     PwmIn ch1;
     PwmIn ch2;
@@ -72,19 +74,23 @@
     //Guardian ap;
     Servo leftservo;
     Servo rightservo;
-    
+    ServoLoop panLoop;
+    ServoLoop tiltLoop;
+
     pixySPI pcam;
-    
+
     Timer timer1;
     Ticker ticker1;
     //RtosTimer controlTimer;
     Ticker tracker;
-    Mutex dutyMutex;
-    Thread dutyThread;
-    static void dutyThreadStarter(void const* p);
+    Mutex trackMutex;
+    //Thread dutyThread;
+    //static void dutyThreadStarter(void const* p);
+    Thread controlThread;
+    static void controlThreadStarter(void const* p);
     void updateDutyCycle();
-    //Thread trackerThread;
-    //static void trackerThreadStarter(void const *p);
+    Thread trackerThread;
+    static void trackerThreadStarter(void const *p);
     float amp;
     float ampCmd;
     float frq;
@@ -104,7 +110,8 @@
     float pitAvg;
     float alPi;
 };
- 
+
+
 #endif
 
 

diff -r 9d75ed1462d6 -r 4f2f441eceec fishgait.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/fishgait.lib	Fri Jul 11 14:30:36 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/jetfishteam/code/fishgait/#d95c40ac8a3e

diff -r 9d75ed1462d6 -r 4f2f441eceec main.cpp
--- a/main.cpp	Fri Jun 06 19:48:01 2014 +0000
+++ b/main.cpp	Fri Jul 11 14:30:36 2014 +0000
@@ -20,12 +20,11 @@
 
     printf("started\n");
     while(true) {
-        pc.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());
+        //pc.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());
 
         //syren.putc(int(mainCtrl.getDutyCycle()));
-        //Thread::wait(100);
-        wait_ms(100);
+        //wait_ms(100);
     }
     //t.stop();
     //mainCtrl.stop();

diff -r 9d75ed1462d6 -r 4f2f441eceec pixy_cam.lib
--- a/pixy_cam.lib	Fri Jun 06 19:48:01 2014 +0000
+++ b/pixy_cam.lib	Fri Jul 11 14:30:36 2014 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/teams/jetfishteam/code/pixy_cam/#a915c5eff55a
+http://mbed.org/teams/jetfishteam/code/pixy_cam/#89b9d1a6457c

diff -r 9d75ed1462d6 -r 4f2f441eceec servoloop.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/servoloop.cpp	Fri Jul 11 14:30:36 2014 +0000
@@ -0,0 +1,29 @@
+#include "servoloop.h"
+ServoLoop::ServoLoop(int32_t pgain, int32_t dgain)
+{
+  m_pos = RCS_CENTER_POS;
+  m_pgain = pgain;
+  m_dgain = dgain;
+  m_prevError = 0x80000000L;
+}
+
+void ServoLoop::update(int32_t error)
+{
+  long int vel;
+  //char buf[32];
+  if (m_prevError!=0x80000000)
+  { 
+    vel = (error*m_pgain + (error - m_prevError)*m_dgain)>>10;
+    //vel = (error*m_pgain + (error - m_prevError)*m_dgain);
+    //sprintf(buf, "%ld\n", vel);
+    //Serial.print(buf);
+    m_pos += vel;
+    if (m_pos>RCS_MAX_POS) 
+      m_pos = RCS_MAX_POS; 
+    else if (m_pos<RCS_MIN_POS) 
+      m_pos = RCS_MIN_POS;
+
+    //cprintf("%d %d %d\n", m_axis, m_pos, vel);
+  }
+  m_prevError = error;
+}
\ No newline at end of file

diff -r 9d75ed1462d6 -r 4f2f441eceec servoloop.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/servoloop.h	Fri Jul 11 14:30:36 2014 +0000
@@ -0,0 +1,20 @@
+#pragma once
+#include "mbed.h"
+//class to aid with vision feedback
+//taken from 
+// https://github.com/charmedlabs/pixy/blob/master/arduino/libraries/Pixy/examples/pantilt/pantilt.ino
+#define RCS_MIN_POS     0L
+#define RCS_MAX_POS     1000L
+#define RCS_CENTER_POS  ((RCS_MAX_POS-RCS_MIN_POS)/2)
+class ServoLoop
+{
+public:
+  ServoLoop(int32_t pgain, int32_t dgain);
+
+  void update(int32_t error);
+   
+  int32_t m_pos;
+  int32_t m_prevError;
+  int32_t m_pgain;
+  int32_t m_dgain;
+};
\ No newline at end of file