Revision 11:8ec915eb70f6, committed 2014-01-31

Comitter:

rkk

Date:

Fri Jan 31 19:36:28 2014 +0000

Parent:

10:d9f1037f0cb0

Child:

12:7eeb29892625

Commit message:

added main controller and control amplitude and frequency of fish tail properly

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MainController.cpp	Fri Jan 31 19:36:28 2014 +0000
@@ -0,0 +1,71 @@
+#include "MainController.h"
+
+MainController::MainController()
+    :ch3(p16,0.054,0.092),
+     ch6(p15,0.055,0.092),
+     mcon(p22, p6, p5)
+{
+    wait_ms(100);
+    vol = 0.0;
+    frq = 10.0;
+    frqMin = 0.5; // hardcoded
+    frqMax = 3.0; //hardcoded
+}
+
+void MainController::control()
+{
+    curTime = timer1.read();
+    if(curTime > 1/frq) {
+        //read new inputs
+        vol = this->calculateVolume();
+        frq = this->calculateFrequency();
+        timer1.reset();
+        curTime = 0.0;
+    }
+    amplitude = vol * frq /frqMax;
+    dutyCycle = amplitude * sin( 2.0 * MATH_PI * frq * curTime );
+    mcon.setpolarspeed(dutyCycle);
+}
+float MainController::calculateFrequency()
+{
+    return ((frqMax - frqMin) * ch3.dutycyclescaledup() + frqMin);    
+}
+
+float MainController::calculateVolume()
+{
+    return (ch6.dutycyclescaledup());
+}
+
+void MainController::start()
+{
+    timer1.start();
+    ticker1.attach(this, &MainController::control,0.0005);
+}
+
+void MainController::stop()
+{
+    timer1.stop();
+    ticker1.detach();
+    mcon.setpolarspeed(0.0);
+}
+
+float MainController::getDutyCycle()
+{
+    return dutyCycle;
+}
+
+float MainController::getAmplitude()
+{
+    return amplitude;
+}
+
+
+float MainController::getFrequency()
+{
+    return frq;
+}
+
+float MainController::getVolume()
+{
+    return vol;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MainController.h	Fri Jan 31 19:36:28 2014 +0000
@@ -0,0 +1,62 @@
+#ifndef MBED_MAINCONTROLLER_H
+#define MBED_MAINCONTROLLER_H
+ 
+#include "mbed.h"
+#include "PwmIn.h"
+#include "motor_controller.h"
+
+#define MATH_PI 3.14159265359
+ 
+/** MainController class to get control inputs and place them onto the system
+ * 
+ * 
+ */
+class MainController {
+public:
+    /** Create a MainController
+     *
+     * @param 
+     */ 
+    MainController() ;
+    
+    /** Start the main controller
+     *
+     * @returns 
+     */
+    void start();
+    
+    float getDutyCycle();
+    float getFrequency();
+    float getVolume();
+    float getAmplitude();
+    
+    /** Stop the main controller
+     *
+     * @returns 
+     */
+    void stop();
+    
+ 
+protected:        
+    void control();
+    float calculateFrequency();
+    float calculateVolume();
+    
+private:
+    PwmIn ch3;
+    PwmIn ch6;
+    PololuMController mcon;
+    Timer timer1;
+    Ticker ticker1;
+    float vol;
+    float frq;
+    float dutyCycle;
+    float curTime;
+    float frqMin;
+    float frqMax;
+    float amplitude;
+};
+ 
+#endif
+
+

--- a/main.cpp	Fri Jan 31 05:18:19 2014 +0000
+++ b/main.cpp	Fri Jan 31 19:36:28 2014 +0000
@@ -1,33 +1,54 @@
 #include "mbed.h"
-#include "motor_controller.h"
+//#include "motor_controller.h"
 #include "guardian.h"
 //#include "IMU.h"
 #include "Servo.h"
 //#include "rtos.h"
 //#include "PwmReader.h"
-#include "PwmIn.h"
+//#include "PwmIn.h"
+#include "MainController.h"
 
-#define VOLUME_MAX 0.4
-#define VOLUME_MIN 0.2
-#define FREQUENCY_MAX 0.4
-#define FREQUENCY_MIN 0.2
-#define RUDDER_MAX 0.4
-#define RUDDER_MIN 0.2
-#define SIZE_ARRAY 500
-
-bool quit=false;
+//bool quit=false;
 
 //InterruptIn event(p16);
-PololuMController mcon(p22, p6, p5);
+
 //Servo servo(p21);
 //Guardian ap(p21, p23, p24, p25, p26, p26);
 //Serial xbee(p13, p14);
 Serial pc(USBTX, USBRX);
-PwmIn ch3(p16,0.054,0.092);
-PwmIn ch6(p15,0.055,0.092);
+//
 Timer t;
+//
+//
+//int counter;
+//int divisions;
+//float vol,frq;
+//PwmOut led(LED1);
 
-float throttle, frequency, rudder;
+//void dosomething ()
+//{
+//    if( counter == divisions) {
+//        ticker1.detach();
+//        vol = ch6.dutycyclescaledup();
+//        frq = ch3.dutycyclescaledup();
+//        counter = 0;
+//        ticker1.attach(&dosomething,1/(frq*float(divisions)));
+//    }
+//    
+//    
+//    
+//    }
+//
+//
+//void startsomething()
+//{
+//    counter = 0;
+//    divisions = 20;
+//
+//    
+//    ticker1.attach(&dosomething,1/(frq*float(divisions)));
+//}
+
 int main()
 {
     //ap.calibrate();
@@ -35,19 +56,25 @@
     //ap.setoff();
     t.start();
     
+    MainController mainCtrl;
+    
+    
+    //startsomething();
+    
+    mainCtrl.start();
+    
+
     while(t.read() < 500) {
+  
         
-        float vol_norm=ch6.dutycyclescaledup();
-        float freq_norm=ch3.dutycyclescaledup();
-           
-
         //mcon.drive_rectangular(t.read(), vol_norm*freq_norm, 3*freq_norm);
         //mcon.drive_sinusoidal(t.read(), vol_norm*freq_norm, 3*freq_norm);
-        float dutycycle = mcon.drive_sinusoidal(t.read(), vol_norm*freq_norm, freq_norm);
-        pc.printf("ch 3: %f, ch 6: %f, dc: %f, asin: %f\n", vol_norm, freq_norm, dutycycle, asin(sin(t.read())) );
+        //float dutycycle = mcon.drive_sinusoidal(t.read(), vol_norm*freq_norm, freq_norm);
+        pc.printf("frq: %f, vol: %f, amp: %f, dut: %f\n", mainCtrl.getFrequency(), mainCtrl.getVolume(), mainCtrl.getAmplitude(),mainCtrl.getDutyCycle() );
         //pc.printf("time: %f\n\r", t.read());
         //pc.printf("throttle: %f frequency: %f rudder: %f\n\r",throttle,frequency,rudder);
-        wait_ms(20);
+        wait_ms(100);
     }
     t.stop();
+    mainCtrl.stop();
 }

--- a/motor_controller.cpp	Fri Jan 31 05:18:19 2014 +0000
+++ b/motor_controller.cpp	Fri Jan 31 19:36:28 2014 +0000
@@ -1,109 +1,101 @@
 #include "motor_controller.h"
-
-float sigm(float input)
-{
-    if (input>0)
-        return 1;
-    else if (input<0)
-        return -1;
-    else
-        return 0;
-}
+//
+//float sigm(float input)
+//{
+//    if (input>0)
+//        return 1;
+//    else if (input<0)
+//        return -1;
+//    else
+//        return 0;
+//}
 
 PololuMController::PololuMController(PinName pwmport, PinName A, PinName B)
-{
-    pwm=new PwmOut(pwmport);
-    outA=new DigitalOut(A);
-    outB=new DigitalOut(B);
-    outA->write(0);
-    outB->write(1);
-    timestamp=0;
-    ome1 = 0.0;
-    amp1 = 0.0;
-    phi1 = 0.0;
-    firstTime = true;
+:pwm(pwmport),outA(A),outB(B){
+    outA.write(0);
+    outB.write(1);
+//    timestamp=0;
+//    ome1 = 0.0;
+//    amp1 = 0.0;
+//    phi1 = 0.0;
+//    firstTime = true;
+
 }
 
-PololuMController::~PololuMController()
-{
-    delete pwm;
-    delete outA;
-    delete outB;
-}
 
-void PololuMController::setspeed(float speed)
-{
-    pwm->write(speed);
-    return;
-}
+//void PololuMController::setspeed(float speed)
+//{
+//    pwm->write(speed);
+//    return;
+//}
 
 void PololuMController::setpolarspeed(float speed)
 {
     if (speed>=0)
     {
-        outA->write(0);
-        outB->write(1);
-        pwm->write(abs(speed));
+        outA.write(0);
+        outB.write(1);
+        pwm.write(abs(speed));
     }
     else
     {
-        outA->write(1);
-        outB->write(0);
-        pwm->write(abs(speed));
+        outA.write(1);
+        outB.write(0);
+        pwm.write(abs(speed));
     }
     return;
 }
 
-void PololuMController::reverse()
-{
-    outA->write(!(outA->read()));
-    outB->write(!(outB->read()));
-    return;
-}
-
-float PololuMController::drive_sinusoidal(float currentTime, float amplitude, float frequency)
-{
-    
-    //convert inputs
-    float amp2;
-    if(amplitude > 1.0) {
-        amp2 = 1.0;
-    } else if(amplitude < 0.0) {
-        amp2 = 0.0;
-    } else {
-        amp2 = amplitude;
-    }
-    float ome2 = 2.0* MATH_PI * frequency;
+//void PololuMController::reverse()
+//{
+//    outA->write(!(outA->read()));
+//    outB->write(!(outB->read()));
+//    return;
+//}
 
-    float dutycycle;
-    float phi2;
-    if (firstTime)
-    {
-        dutycycle = amp2*sin(ome2 * currentTime);
-        firstTime = false;
-    }
-    else if(amp2 > 0.0) {
-        phi2 = asin(amp1 * sin(ome1 * currentTime + phi1) / amp2 ) - (ome2*currentTime);
-        dutycycle = amp2*sin(ome2 * currentTime + phi2);
-        phi1 = phi2;
-    } else {
-        dutycycle = 0.0;
-    }
-    setpolarspeed(dutycycle);
-
-    //set previous values
-    ome1 = ome2;
-    amp1 = amp2;
-
-    return dutycycle;
-}
-
-void PololuMController::drive_rectangular(float currentTime, float amplitude, float frequency)
-{    
-    //setpolarspeed(dutyCycle*sin( 2.0* MATH_PI* f * currentTime));
-    float sinRes = sin( 2.0* MATH_PI* frequency * currentTime);
-    
-    float dutycycle = amplitude * sigm( sinRes);
-    setpolarspeed(dutycycle);
-    return;
-}
+//float PololuMController::drive_sinusoidal(float currentTime, float amplitude, float frequency)
+//{
+//   
+//    //convert inputs
+//    float amp2;
+//    if(amplitude > 1.0) {
+//        amp2 = 1.0;
+//    } else if(amplitude < 0.0) {
+//        amp2 = 0.0;
+//    } else {
+//        amp2 = amplitude;
+//    }
+//    float ome2 = 2.0* MATH_PI * frequency;
+//
+//    float dutycycle;
+//    float phi2;
+//    if (firstTime)
+//    {
+//        dutycycle = amp2*sin(ome2 * currentTime);
+//        firstTime = false;
+//    }
+//    else if(amp2 > 0.0) {
+//        phi2 = asin(amp1 * sin(ome1 * currentTime + phi1) / amp2 ) - (ome2*currentTime);
+//        dutycycle = amp2*sin(ome2 * currentTime + phi2);
+//        phi1 = phi2;
+//    } else {
+//        dutycycle = 0.0;
+//    }
+//    setpolarspeed(dutycycle);
+//
+//    //set previous values
+//    ome1 = ome2;
+//    amp1 = amp2;
+//
+//    return dutycycle;
+//}
+//
+//void PololuMController::drive_rectangular(float currentTime, float amplitude, float frequency)
+//{    
+//    //setpolarspeed(dutyCycle*sin( 2.0* MATH_PI* f * currentTime));
+//    float sinRes = sin( 2.0* MATH_PI* frequency * currentTime);
+//    
+//    float dutycycle = amplitude * sigm( sinRes);
+//    setpolarspeed(dutycycle);
+//    return;
+//}

--- a/motor_controller.h	Fri Jan 31 05:18:19 2014 +0000
+++ b/motor_controller.h	Fri Jan 31 19:36:28 2014 +0000
@@ -1,6 +1,6 @@
 #pragma once
 #include "mbed.h"
-#define MATH_PI 3.14159265359
+
 #define FREQ_MIN 0.5  //Hz
 #define FREQ_MAX 3  //Hz
 
@@ -9,22 +9,24 @@
 class PololuMController
 {
     private:
-    PwmOut* pwm;
-    DigitalOut* outA;
-    DigitalOut* outB;
-    float timestamp;
+    PwmOut pwm;
+    DigitalOut outA;
+    DigitalOut outB;
+    //float timestamp;
     
-    //for driving
-    float phi1, ome1,amp1;
-    bool firstTime;
+//    //for driving
+//    float phi1, ome1,amp1;
+//    bool firstTime;
+//    
+//    Timer t;
+    
     
     public:
     PololuMController();
     PololuMController(PinName pwmport, PinName A, PinName B);
-    ~PololuMController();
-    void setspeed(float speed);         //0 to 1
+    //void setspeed(float speed);         //0 to 1
     void setpolarspeed(float speed);    //-1 to 1
-    void reverse();                     //only works on non-polar speed
-    float drive_sinusoidal(float currentTime, float amplitude, float frequency);
-    void drive_rectangular(float currentTime, float amplitude, float frequency);
+    //void reverse();                     //only works on non-polar speed
+    //float drive_sinusoidal(float currentTime, float amplitude, float frequency);
+    //void drive_rectangular(float currentTime, float amplitude, float frequency);
 };
\ No newline at end of file