Revision 8:0574a5db1fc4, committed 2014-01-31

Comitter:

rkk

Date:

Fri Jan 31 04:33:44 2014 +0000

Parent:

7:e005cfaff8d1

Child:

9:8dd7a76756e2

Commit message:

updated the motor controller and the pwmin method

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PwmIn.cpp	Fri Jan 31 04:33:44 2014 +0000
@@ -0,0 +1,55 @@
+#include "PwmIn.h"
+ 
+PwmIn::PwmIn(PinName p, float dutyMin, float dutyMax) 
+    : _p(p) {
+    _p.rise(this, &PwmIn::rise);
+    _p.fall(this, &PwmIn::fall);
+    _period = 20000;
+    _pulsewidth = 0;
+    _t.start();
+    _risen = false;
+    _dutyMin = dutyMin;
+    _dutyMax = dutyMax;
+    _dutyDelta = dutyMax - dutyMin;
+
+}
+ 
+float PwmIn::period() {
+    return float(_period)/1000000;
+}
+ 
+float PwmIn::pulsewidth() {
+    return float(_pulsewidth)/1000000;
+}
+ 
+float PwmIn::dutycycle() {
+    return float(_pulsewidth) / float(_period);
+}
+
+float PwmIn::dutycyclescaledup() {
+    float duty = float(_pulsewidth) / float(_period);
+    float dutyAdjusted = (duty > _dutyMin) ? ((duty-_dutyMin)/_dutyDelta) : 0.0;
+    return (dutyAdjusted < 1.0) ? dutyAdjusted : 1.0;
+     
+}
+ 
+void PwmIn::rise()
+{
+    _tmp = _t.read_us();
+    if(_tmp > 19000) {
+        _period = _tmp;
+        _risen = true;
+        _t.reset();
+    }
+}
+
+void PwmIn::fall()
+{
+    _tmp = _t.read_us();
+    if(_tmp > 1000 && _tmp < 1950 && _risen == true) {
+        _pulsewidth = _tmp;
+        _risen = false;
+    }
+}
+
+    
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PwmIn.h	Fri Jan 31 04:33:44 2014 +0000
@@ -0,0 +1,55 @@
+
+#ifndef MBED_PWMIN_H
+#define MBED_PWMIN_H
+ 
+#include "mbed.h"
+ 
+/** PwmIn class to read PWM inputs
+ * 
+ * Uses InterruptIn to measure the changes on the input
+ * and record the time they occur
+ *
+ * @note uses InterruptIn, so not available on p19/p20
+ */
+class PwmIn {
+public:
+    /** Create a PwmIn
+     *
+     * @param p The pwm input pin (must support InterruptIn)
+     */ 
+    PwmIn(PinName p, float dutyMin, float dutyMax) ;
+    
+    /** Read the current period
+     *
+     * @returns the period in seconds
+     */
+    float period();
+    
+    /** Read the current pulsewidth
+     *
+     * @returns the pulsewidth in seconds
+     */
+    float pulsewidth();
+    
+    /** Read the current dutycycle
+     *
+     * @returns the dutycycle as a percentage, represented between 0.0-1.0
+     */
+    float dutycycle();
+    
+    float dutycyclescaledup();
+ 
+protected:        
+    void rise();
+    void fall();
+    
+    InterruptIn _p;
+    Timer _t;
+    int _pulsewidth, _period;
+    int _tmp;
+    float _dutyMin, _dutyMax, _dutyDelta;
+    
+    bool _risen;
+};
+ 
+#endif
\ No newline at end of file

--- a/PwmReader.h	Thu Jan 30 02:04:23 2014 +0000
+++ b/PwmReader.h	Fri Jan 31 04:33:44 2014 +0000
@@ -13,6 +13,8 @@
     protected:
     void pwmRise();    
     void pwmFall();    
+    void pwmRise2();    
+    void pwmFall2(); 
     
     private:
     InterruptIn* di;

--- a/main.cpp	Thu Jan 30 02:04:23 2014 +0000
+++ b/main.cpp	Fri Jan 31 04:33:44 2014 +0000
@@ -4,25 +4,27 @@
 //#include "IMU.h"
 #include "Servo.h"
 //#include "rtos.h"
-#include "PwmReader.h"
+//#include "PwmReader.h"
+#include "PwmIn.h"
 
-#define THROTTLE_MAX 0.4
-#define THROTTLE_MIN 0.2
+#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;
 
-InterruptIn event(p16);
+//InterruptIn event(p16);
 PololuMController mcon(p22, p6, p5);
-Servo servo(p21);
-Guardian ap(p21, p23, p24, p25, p26, p26);
-Serial xbee(p13, p14);
+//Servo servo(p21);
+//Guardian ap(p21, p23, p24, p25, p26, p26);
+//Serial xbee(p13, p14);
 Serial pc(USBTX, USBRX);
-PwmReader ch3(p16, 0.054, 0.094);
-PwmReader ch6(p15, 0.054, 0.094);
+PwmIn ch3(p16,0.054,0.092);
+PwmIn ch6(p15,0.055,0.092);
 Timer t;
 
 float throttle, frequency, rudder;
@@ -30,19 +32,23 @@
 {
     //ap.calibrate();
     //ap.set2D();
-    ap.setoff();
+    //ap.setoff();
     t.start();
-    while(1) {
+    
+    while(t.read() < 500) {
         
-        float vol_norm=ch3.getDuty();
-        float freq_norm=ch6.getDuty();
+        float vol_norm=ch6.dutycyclescaledup();
+        float freq_norm=ch3.dutycyclescaledup();
+        
         
         pc.printf("channel 3: %f, channel 6: %f\n", vol_norm, freq_norm);
+
         //mcon.drive_rectangular(t.read(), vol_norm*freq_norm, 3*freq_norm);
+        //mcon.drive_sinusoidal(t.read(), vol_norm*freq_norm, 3*freq_norm);
         mcon.drive_sinusoidal(t.read(), vol_norm*freq_norm, 3*freq_norm);
         //pc.printf("time: %f\n\r", t.read());
         //pc.printf("throttle: %f frequency: %f rudder: %f\n\r",throttle,frequency,rudder);
-        //wait_ms(10);
+        wait_ms(1);
     }
     t.stop();
 }

--- a/motor_controller.cpp	Thu Jan 30 02:04:23 2014 +0000
+++ b/motor_controller.cpp	Fri Jan 31 04:33:44 2014 +0000
@@ -18,6 +18,9 @@
     outA->write(0);
     outB->write(1);
     timestamp=0;
+    ome1 = 0.0;
+    amp1 = 0.0;
+    phi1 = 0.0;
 }
 
 PololuMController::~PololuMController()
@@ -57,11 +60,14 @@
     return;
 }
 
-void PololuMController::drive_sinusoidal(float currentTime, float dutyCycle, float frequency)
+void PololuMController::drive_sinusoidal(float currentTime, float amplitude, float frequency)
 {
+   float ome2 = 2.0* MATH_PI * frequency;
+   float divisor = (ome2*currentTime);
+   float phi2 = asin(amp1/amplitude*sin(ome1 * currentTime + phi1))/divisor;
    
-    setpolarspeed(dutyCycle*sin( 2.0* MATH_PI* frequency * currentTime));
-    return;
+   setpolarspeed(amplitude*sin(ome2 * currentTime + phi2));
+   return;
 }
 
 void PololuMController::drive_rectangular(float currentTime, float amplitude, float frequency)

--- a/motor_controller.h	Thu Jan 30 02:04:23 2014 +0000
+++ b/motor_controller.h	Fri Jan 31 04:33:44 2014 +0000
@@ -13,6 +13,11 @@
     DigitalOut* outA;
     DigitalOut* outB;
     float timestamp;
+    
+    //for driving
+    float phi1, ome1,amp1;
+    
+    
     public:
     PololuMController();
     PololuMController(PinName pwmport, PinName A, PinName B);
@@ -20,6 +25,6 @@
     void setspeed(float speed);         //0 to 1
     void setpolarspeed(float speed);    //-1 to 1
     void reverse();                     //only works on non-polar speed
-    void drive_sinusoidal(float currentTime, float dutyCycle, float frequency);
+    void drive_sinusoidal(float currentTime, float amplitude, float frequency);
     void drive_rectangular(float currentTime, float amplitude, float frequency);
 };
\ No newline at end of file