Diff: Motorregler_simple.cpp

Revision:

6:f97371e6bc3e

Parent:

5:a68e6c550e4b

--- a/Motorregler_simple.cpp	Wed Sep 10 12:51:11 2014 +0000
+++ b/Motorregler_simple.cpp	Wed Sep 10 12:59:51 2014 +0000
@@ -3,41 +3,89 @@
 
 PwmOut Motroregler_PWM(p22);
 
+volatile int Drehzeit_counter = 0;
+#define DREHZEIT_SIZE 3
+volatile int Drehzeit[DREHZEIT_SIZE];
 int void()
 {
 
 //***************************************************************************************************
 //Control Motor rpm
- 
-int rpm_control(float motor_n_cmd, float motor_n_cur)
-{
- 
-    static int motor_pwm_cmd_last = 900;
-    //static float motor_n_last = 0;
- 
-    if (motor_n_cmd < 1.0) {
-        Motroregler_PWM.pulsewidth_us(900);
-        motor_pwm_cmd_last = 900;
+
+    int rpm_control(float motor_n_cmd, float motor_n_cur) {
+
+        static int motor_pwm_cmd_last = 900;
+        //static float motor_n_last = 0;
+
+        if (motor_n_cmd < 1.0) {
+            Motroregler_PWM.pulsewidth_us(900);
+            motor_pwm_cmd_last = 900;
 //        motor_n_last = 0;
+            return 1;
+        }
+
+        float motor_n_dif = motor_n_cmd - motor_n_cur;
+
+        int motor_pwm_cmd = (int)(motor_pwm_cmd_last + motor_n_dif * 0.6 + 0.5); // round() ... works only for positive values
+
+        pc.printf("cmd: %7.2f, cur: %7.2f, dif: %7.2f, motor_pwm_cmd: %4d, motor_pwm_dif: %4d, DMS: %f\n\r",
+                  motor_n_cmd*60, motor_n_cur*60, motor_n_dif*60, motor_pwm_cmd, motor_pwm_cmd-motor_pwm_cmd_last, ((int)DMS_0.read_u16())/65536.0*3.3);
+
+        if (motor_pwm_cmd > 1900) motor_pwm_cmd = 1900;
+        else if (motor_pwm_cmd < 1010) motor_pwm_cmd = 1005;
+
+        Motroregler_PWM.pulsewidth_us(motor_pwm_cmd);
+        motor_pwm_cmd_last = motor_pwm_cmd;
+
         return 1;
     }
- 
-    float motor_n_dif = motor_n_cmd - motor_n_cur;
- 
-    int motor_pwm_cmd = (int)(motor_pwm_cmd_last + motor_n_dif * 0.6 + 0.5); // round() ... works only for positive values
- 
-    pc.printf("cmd: %7.2f, cur: %7.2f, dif: %7.2f, motor_pwm_cmd: %4d, motor_pwm_dif: %4d, DMS: %f\n\r",
-              motor_n_cmd*60, motor_n_cur*60, motor_n_dif*60, motor_pwm_cmd, motor_pwm_cmd-motor_pwm_cmd_last, ((int)DMS_0.read_u16())/65536.0*3.3);
- 
-    if (motor_pwm_cmd > 1900) motor_pwm_cmd = 1900;
-    else if (motor_pwm_cmd < 1010) motor_pwm_cmd = 1005;
- 
-    Motroregler_PWM.pulsewidth_us(motor_pwm_cmd);
-    motor_pwm_cmd_last = motor_pwm_cmd;
- 
-    return 1;
- }
- 
+
 //***************************************************************************************************
 //Control MOSFET pwm
+    int brk_mosfet_control(float mosfet_pwm_cmd, float mosfet_pwm_cur) {
+
+        static int mosfet_pwm_cmd_last = 0;
+
+
+        if (mosfet_pwm_cmd < 1.0) {
+            Bremsenstrom_MOSFET.pulsewidth_us(00);
+            mosfet_pwm_cmd_last = 0;
+
+            return 1;
+        }
+
+        float mosfet_pwm_dif = mosfet_pwm_cmd - mosfet_pwm_cur;
+
+        // int mosfet_pwm_cmd = (int)(mosfet_pwm_cmd_last + mosfet_pwm_dif * 0.6 + 0.5); // round() ... works only for positive values
+
+        pc.printf("cmd: %7.2f, cur: %7.2f, dif: %7.2f, mosfet_pwm_cmd: %4d, mosfet_pwm_dif: %4d",
+                  mosfet_pwm_cmd, mosfet_pwm_cur, mosfet_pwm_dif, mosfet_pwm_cmd, mosfet_pwm_cmd-mosfet_pwm_cmd_last);
+
+        if (mosfet_pwm_cmd > 20000) mosfet_pwm_cmd = 20000;
+        else if (mosfet_pwm_cmd < 0) mosfet_pwm_cmd = 0;
+
+        Bremsenstrom_MOSFET.pulsewidth_us(mosfet_pwm_cmd);
+
+        mosfet_pwm_cmd_last = mosfet_pwm_cmd;
+
+        return 1;
+    }
+    //***************************************************************************************************
+//Calculate rpm
+    void Motor_drehzahl() {
+        static bool first_run = true;
+//   Umlaufzeit.stop();
+        int tmp = Umlaufzeit.read_us();
+        if (first_run) {
+            Umlaufzeit.start();
+            first_run = false;
+            return;
+        }
+        if (tmp < 1000) return;
+
+        // Cache last 3 values for averaging
+        Drehzeit[Drehzeit_counter % DREHZEIT_SIZE] = tmp;
+        ++Drehzeit_counter;
+        Umlaufzeit.reset();
+    }
 }
\ No newline at end of file