Revision 6:a4d6f3e4bf28, committed 2014-01-29

Comitter:

rkk

Date:

Wed Jan 29 05:04:50 2014 +0000

Parent:

5:090ef6275773

Child:

7:e005cfaff8d1

Commit message:

added pam ira

Changed in this revision

--- a/PwmReader.cpp	Tue Jan 28 23:59:21 2014 +0000
+++ b/PwmReader.cpp	Wed Jan 29 05:04:50 2014 +0000
@@ -1,10 +1,16 @@
 #include "PwmReader.h"
 
 //Constructors
-PwmReader::PwmReader(PinName pwmInPort)
+PwmReader::PwmReader(PinName pwmInPort, float min, float max)
 {
+    if (min > max)
+    {
+        error("PwmReader min value greater than max value!");
+    }
+    
     di = new InterruptIn(pwmInPort);
-    
+    pwmMin = min;
+    pwmMax = max;
     lastRise = 0;
     period = 0;
     duty = 0.0;
@@ -20,7 +26,9 @@
 // public methods
 float PwmReader::getDuty()
 {
-    return duty;
+    float smallDelta = (duty > pwmMin) ? (duty - pwmMin) : 0.0;
+    float dutyAdjusted = smallDelta / (pwmMax-pwmMin);
+    return dutyAdjusted;
 }
 
 // private methods
@@ -36,9 +44,9 @@
 
 void PwmReader::pwmFall()
 {
-    int fallTime = t.read_us();
+    int fall = t.read_us();
     if(period > 0 ) {
-        int delta = fallTime - lastRise;
-        duty = float(delta)/float(period);
+        int delta = fall - lastRise;
+        duty = float(delta)/float(period); 
     }
 }
\ No newline at end of file

--- a/PwmReader.h	Tue Jan 28 23:59:21 2014 +0000
+++ b/PwmReader.h	Wed Jan 29 05:04:50 2014 +0000
@@ -5,7 +5,7 @@
 {
     public:
     PwmReader();
-    PwmReader(PinName pwmInPort);
+    PwmReader(PinName pwmInPort, float min = 0.0, float max = 1.0);
     ~PwmReader();
     
     float getDuty();         //0 to 1
@@ -14,10 +14,13 @@
     void pwmRise();    //0 to 1
     void pwmFall();    //-1 to 1
     
+    private:
     InterruptIn* di;
     int lastRise;
     int period;
     float duty;
-    Timer t; 
+    Timer t;
+    float pwmMin;
+    float pwmMax; 
     
 };
\ No newline at end of file

--- a/main.cpp	Tue Jan 28 23:59:21 2014 +0000
+++ b/main.cpp	Wed Jan 29 05:04:50 2014 +0000
@@ -5,6 +5,14 @@
 #include "Servo.h"
 #include "rtos.h"
 #include "PwmReader.h"
+
+#define THROTTLE_MAX 0.4
+#define THROTTLE_MIN 0.2
+#define FREQUENCY_MAX 0.4
+#define FREQUENCY_MIN 0.2
+#define RUDDER_MAX 0.4
+#define RUDDER_MIN 0.2
+
 bool quit=false;
 
 PololuMController mcon(p22, p6, p5);
@@ -13,22 +21,25 @@
 Serial xbee(p13, p14);
 Serial pc(USBTX, USBRX);
 
+float throttle, frequency, rudder;
 
 //IMU imu(0.1, 0.3, 0.005, 0.005);
 void motor_thread(void const *args) {
     
     Timer t;
     t.start();
-    PwmReader thrDutyCycl(p7);
-    
-    PwmReader freqDutyCycl(p8);   
+    PwmReader thrDutyCycl(p7, THROTTLE_MIN, THROTTLE_MAX);
+    PwmReader freqDutyCycl(p8, FREQUENCY_MIN, FREQUENCY_MAX);   
+    PwmReader rudDutyCycl(p9, RUDDER_MIN, RUDDER_MAX);
     
     while (quit==false) {     
-        float td = thrDutyCycl.getDuty();
-        float fd = freqDutyCycl.getDuty();
-        pc.printf("throttle: %f frequency: %f\n",td,fd);
-        mcon.drive_sinusoidal(t.read(), 1, 2*3.141 ,0); // (time,amplitude,frequency,freqoffset)
-        Thread::wait(10);
+        throttle = thrDutyCycl.getDuty();
+        frequency = freqDutyCycl.getDuty();
+        rudder = rudDutyCycl.getDuty();
+        
+        
+        mcon.drive_sinusoidal(t.read(), throttle, frequency); // (time,amplitude,frequency,freqoffset)
+        Thread::wait(5);
     }
     t.stop();
 }
@@ -48,8 +59,10 @@
 //            pc.puts(buf);
 //        }
 //        memset(buf, 0, 128);
+         
+         //print throttle and frequency to the terminal
+          pc.printf("throttle: %f frequency: %f rudder: %f\n\r",throttle,frequency,rudder);
                
-        
-        Thread::wait(10);
+          Thread::wait(500);
     }
 }

--- a/motor_controller.cpp	Tue Jan 28 23:59:21 2014 +0000
+++ b/motor_controller.cpp	Wed Jan 29 05:04:50 2014 +0000
@@ -47,8 +47,11 @@
     return;
 }
 
-void PololuMController::drive_sinusoidal(float cur_time, float a, float w, float phi)
+void PololuMController::drive_sinusoidal(float currentTime, float dutyCycle, float frequency)
 {
-    setpolarspeed(a*sin(w*cur_time+phi));
+    //convert frequency form 0.0 to 1.0
+    float f = (FREQ_MAX - FREQ_MIN) * f + FREQ_MIN; 
+    
+    setpolarspeed(dutyCycle*sin( 2.0* MATH_PI* f * currentTime));
     return;
 }

--- a/motor_controller.h	Tue Jan 28 23:59:21 2014 +0000
+++ b/motor_controller.h	Wed Jan 29 05:04:50 2014 +0000
@@ -1,5 +1,8 @@
 #pragma once
 #include "mbed.h"
+#define MATH_PI 3.14159265359
+#define FREQ_MIN 0.5  //Hz
+#define FREQ_MAX 8  //Hz
 
 class PololuMController
 {
@@ -15,5 +18,5 @@
     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 cur_time, float a, float w, float phi); //a*sin(w*cur_time+phi)
+    void drive_sinusoidal(float currentTime, float dutyCycle, float frequency);
 };
\ No newline at end of file