Revision 12:7eeb29892625, committed 2014-02-01

Comitter:

rkk

Date:

Sat Feb 01 00:03:40 2014 +0000

Parent:

11:8ec915eb70f6

Child:

13:5ed8fd870723

Commit message:

first design to put to water

Changed in this revision

--- a/MainController.cpp	Fri Jan 31 19:36:28 2014 +0000
+++ b/MainController.cpp	Sat Feb 01 00:03:40 2014 +0000
@@ -1,15 +1,19 @@
 #include "MainController.h"
 
 MainController::MainController()
-    :ch3(p16,0.054,0.092),
-     ch6(p15,0.055,0.092),
-     mcon(p22, p6, p5)
+    :ch3(p16,0.054,0.092), // frequency
+     ch4(p17,0.055,0.092), //rudder
+     ch6(p15,0.055,0.092), //volume
+     mcon(p22, p6, p5),
+     ap(p25, p26)
 {
-    wait_ms(100);
+    wait_ms(50);
     vol = 0.0;
     frq = 10.0;
+    rud = 0.5;
     frqMin = 0.5; // hardcoded
     frqMax = 3.0; //hardcoded
+    frqmxsqrt = sqrt(frqMax);
 }
 
 void MainController::control()
@@ -22,10 +26,15 @@
         timer1.reset();
         curTime = 0.0;
     }
-    amplitude = vol * frq /frqMax;
+    
+    rud = this->calculateRudder(); //not used right now
+    amplitude = vol * sqrt(frq)/frqmxsqrt;
     dutyCycle = amplitude * sin( 2.0 * MATH_PI * frq * curTime );
+    //sigm for alternative driving mode
     mcon.setpolarspeed(dutyCycle);
 }
+
+
 float MainController::calculateFrequency()
 {
     return ((frqMax - frqMin) * ch3.dutycyclescaledup() + frqMin);    
@@ -36,10 +45,20 @@
     return (ch6.dutycyclescaledup());
 }
 
+float MainController::calculateRudder()
+{
+    return (ch4.dutycyclescaledup());
+}
+
 void MainController::start()
 {
     timer1.start();
-    ticker1.attach(this, &MainController::control,0.0005);
+    ticker1.attach(this, &MainController::control,0.001);
+    //Autopilot guardian
+    //ap.calibrate();
+    //ap.set2D();
+    ap.setoff();
+    
 }
 
 void MainController::stop()
@@ -69,3 +88,19 @@
 {
     return vol;
 }
+
+float MainController::getRudder()
+{
+    return rud;
+}
+
+//
+//float MainController::sigm(float input)
+//{
+//    if (input>0)
+//        return 1;
+//    else if (input<0)
+//        return -1;
+//    else
+//        return 0;
+//}
\ No newline at end of file

--- a/MainController.h	Fri Jan 31 19:36:28 2014 +0000
+++ b/MainController.h	Sat Feb 01 00:03:40 2014 +0000
@@ -4,6 +4,9 @@
 #include "mbed.h"
 #include "PwmIn.h"
 #include "motor_controller.h"
+#include "guardian.h"
+//#include "IMU.h"
+
 
 #define MATH_PI 3.14159265359
  
@@ -29,6 +32,7 @@
     float getFrequency();
     float getVolume();
     float getAmplitude();
+    float getRudder();
     
     /** Stop the main controller
      *
@@ -41,11 +45,16 @@
     void control();
     float calculateFrequency();
     float calculateVolume();
+    float calculateRudder();
     
 private:
     PwmIn ch3;
+    PwmIn ch4;
     PwmIn ch6;
     PololuMController mcon;
+    Guardian ap;
+    
+    
     Timer timer1;
     Ticker ticker1;
     float vol;
@@ -55,6 +64,8 @@
     float frqMin;
     float frqMax;
     float amplitude;
+    float rud;
+    float frqmxsqrt;
 };
  
 #endif

--- a/guardian.cpp	Fri Jan 31 19:36:28 2014 +0000
+++ b/guardian.cpp	Sat Feb 01 00:03:40 2014 +0000
@@ -1,43 +1,29 @@
 #include "guardian.h"
 
-Guardian::Guardian(PinName ailpin, PinName modpin, PinName elvpin, PinName rudpin, PinName auxpin, PinName gainpin)
+Guardian::Guardian(PinName modpin, PinName gainpin)
+:mod(modpin),
+gain(gainpin)
 {
-    ail=new Servo(ailpin);
-    mod=new Servo(modpin);
-    elv=new Servo(elvpin);
-    /*rud=new Servo(rudpin);
-    aux=new Servo(auxpin);
-    */
-    gain=new Servo(gainpin);
-    mod->write(0.5); //set autopilot to off
-    ail->write(0.5);
-    elv->write(0.5);
-    /*rud->write(0.5);
-    aux->write(0.00);
-    */
-    gain->write(1.00);
-}
 
-Guardian::~Guardian()
-{
-    delete ail, mod, elv, rud, aux, gain;
+    mod.write(0.5); //set autopilot to off
+    gain.write(1.00);
 }
 
 void Guardian::set3D() //set autopilot to 3D
 {
-    mod->write(1.00);
+    mod.write(1.00);
     return;
 }
 
 void Guardian::set2D() //set autopilot 2D
 {
-    mod->write(0.00);
+    mod.write(0.00);
     return;
 }
 
 void Guardian::setoff()
 {
-    mod->write(0.5);
+    mod.write(0.5);
     return;
 }
 
@@ -53,32 +39,14 @@
     return;
 }
 
-void Guardian::setail(float val)
-{
-    ail->write(val);
-    return;
-}
-
 void Guardian::setmod(float val)
 {
-    mod->write(val);
+    mod.write(val);
     return;
 }
 
-void Guardian::setelv(float val)
-{
-    elv->write(val);
-    return;
-}
-
-void Guardian::setrud(float val)
+void Guardian::setgain(float val)
 {
-    rud->write(val);
-    return;
-}
-
-void Guardian::setaux(float val)
-{
-    aux->write(val);
+    gain.write(val);
     return;
 }
\ No newline at end of file

--- a/guardian.h	Fri Jan 31 19:36:28 2014 +0000
+++ b/guardian.h	Sat Feb 01 00:03:40 2014 +0000
@@ -5,22 +5,13 @@
 class Guardian
 {
 private:
-Servo* ail;
-Servo* mod;
-Servo* elv;
-Servo* rud;
-Servo* aux;
-Servo* gain;
+    Servo mod, gain;
 public:
-Guardian(PinName ailpin, PinName modpin, PinName elvpin, PinName rudpin, PinName auxpin, PinName gainpin);
-~Guardian();
-void set2D();
-void set3D();
-void setoff();
-void calibrate();
-void setail(float val);
-void setmod(float val);
-void setelv(float val);
-void setrud(float val);
-void setaux(float val);
+    Guardian(PinName modpin,PinName gainpin);
+    void set2D();
+    void set3D();
+    void setoff();
+    void calibrate();
+    void setmod(float val);
+    void setgain(float val);
 };
\ No newline at end of file

--- a/main.cpp	Fri Jan 31 19:36:28 2014 +0000
+++ b/main.cpp	Sat Feb 01 00:03:40 2014 +0000
@@ -1,80 +1,28 @@
 #include "mbed.h"
-//#include "motor_controller.h"
-#include "guardian.h"
-//#include "IMU.h"
 #include "Servo.h"
 //#include "rtos.h"
 //#include "PwmReader.h"
 //#include "PwmIn.h"
 #include "MainController.h"
 
-//bool quit=false;
-
-//InterruptIn event(p16);
-
-//Servo servo(p21);
-//Guardian ap(p21, p23, p24, p25, p26, p26);
-//Serial xbee(p13, p14);
 Serial pc(USBTX, USBRX);
-//
 Timer t;
-//
-//
-//int counter;
-//int divisions;
-//float vol,frq;
-//PwmOut led(LED1);
-
-//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();
-    //ap.set2D();
-    //ap.setoff();
     t.start();
     
     MainController mainCtrl;
-    
-    
-    //startsomething();
-    
+       
     mainCtrl.start();
-    
 
-    while(t.read() < 500) {
+    while(true) {
   
-        
-        //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("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);
+        pc.printf("frq: %f, vol: %f, amp: %f, rud: %f, dut: %f, t: %f\n",
+         mainCtrl.getFrequency(), mainCtrl.getVolume(), mainCtrl.getAmplitude(), mainCtrl.getRudder(), mainCtrl.getDutyCycle(),t.read());
+
         wait_ms(100);
     }
-    t.stop();
-    mainCtrl.stop();
+    //t.stop();
+    //mainCtrl.stop();
 }

--- a/motor_controller.cpp	Fri Jan 31 19:36:28 2014 +0000
+++ b/motor_controller.cpp	Sat Feb 01 00:03:40 2014 +0000
@@ -1,101 +1,22 @@
 #include "motor_controller.h"
-//
-//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(pwmport),outA(A),outB(B){
+    :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;
-
 }
 
-
-//void PololuMController::setspeed(float speed)
-//{
-//    pwm->write(speed);
-//    return;
-//}
-
 void PololuMController::setpolarspeed(float speed)
 {
-    if (speed>=0)
-    {
+    if (speed>=0) {
         outA.write(0);
         outB.write(1);
         pwm.write(abs(speed));
-    }
-    else
-    {
+    } else {
         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;
-//
-//    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 19:36:28 2014 +0000
+++ b/motor_controller.h	Sat Feb 01 00:03:40 2014 +0000
@@ -12,21 +12,10 @@
     PwmOut pwm;
     DigitalOut outA;
     DigitalOut outB;
-    //float timestamp;
-    
-//    //for driving
-//    float phi1, ome1,amp1;
-//    bool firstTime;
-//    
-//    Timer t;
     
     
     public:
     PololuMController();
     PololuMController(PinName pwmport, PinName A, PinName B);
-    //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);
 };
\ No newline at end of file