Motorregler_simple.cpp

00001 #include "mbed.h"
00002 
00003 
00004 PwmOut Motroregler_PWM(p22);
00005 
00006 volatile int Drehzeit_counter = 0;
00007 #define DREHZEIT_SIZE 3
00008 volatile int Drehzeit[DREHZEIT_SIZE];
00009 int void()
00010 {
00011 
00012 //***************************************************************************************************
00013 //Control Motor rpm
00014 
00015     int rpm_control(float motor_n_cmd, float motor_n_cur) {
00016 
00017         static int motor_pwm_cmd_last = 900;
00018         //static float motor_n_last = 0;
00019 
00020         if (motor_n_cmd < 1.0) {
00021             Motroregler_PWM.pulsewidth_us(900);
00022             motor_pwm_cmd_last = 900;
00023 //        motor_n_last = 0;
00024             return 1;
00025         }
00026 
00027         float motor_n_dif = motor_n_cmd - motor_n_cur;
00028 
00029         int motor_pwm_cmd = (int)(motor_pwm_cmd_last + motor_n_dif * 0.6 + 0.5); // round() ... works only for positive values
00030 
00031         pc.printf("cmd: %7.2f, cur: %7.2f, dif: %7.2f, motor_pwm_cmd: %4d, motor_pwm_dif: %4d, DMS: %f\n\r",
00032                   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);
00033 
00034         if (motor_pwm_cmd > 1900) motor_pwm_cmd = 1900;
00035         else if (motor_pwm_cmd < 1010) motor_pwm_cmd = 1005;
00036 
00037         Motroregler_PWM.pulsewidth_us(motor_pwm_cmd);
00038         motor_pwm_cmd_last = motor_pwm_cmd;
00039 
00040         return 1;
00041     }
00042 
00043 //***************************************************************************************************
00044 //Control MOSFET pwm
00045     int brk_mosfet_control(float mosfet_pwm_cmd, float mosfet_pwm_cur) {
00046 
00047         static int mosfet_pwm_cmd_last = 0;
00048 
00049 
00050         if (mosfet_pwm_cmd < 1.0) {
00051             Bremsenstrom_MOSFET.pulsewidth_us(00);
00052             mosfet_pwm_cmd_last = 0;
00053 
00054             return 1;
00055         }
00056 
00057         float mosfet_pwm_dif = mosfet_pwm_cmd - mosfet_pwm_cur;
00058 
00059         // int mosfet_pwm_cmd = (int)(mosfet_pwm_cmd_last + mosfet_pwm_dif * 0.6 + 0.5); // round() ... works only for positive values
00060 
00061         pc.printf("cmd: %7.2f, cur: %7.2f, dif: %7.2f, mosfet_pwm_cmd: %4d, mosfet_pwm_dif: %4d",
00062                   mosfet_pwm_cmd, mosfet_pwm_cur, mosfet_pwm_dif, mosfet_pwm_cmd, mosfet_pwm_cmd-mosfet_pwm_cmd_last);
00063 
00064         if (mosfet_pwm_cmd > 20000) mosfet_pwm_cmd = 20000;
00065         else if (mosfet_pwm_cmd < 0) mosfet_pwm_cmd = 0;
00066 
00067         Bremsenstrom_MOSFET.pulsewidth_us(mosfet_pwm_cmd);
00068 
00069         mosfet_pwm_cmd_last = mosfet_pwm_cmd;
00070 
00071         return 1;
00072     }
00073     //***************************************************************************************************
00074 //Calculate rpm
00075     void Motor_drehzahl() {
00076         static bool first_run = true;
00077 //   Umlaufzeit.stop();
00078         int tmp = Umlaufzeit.read_us();
00079         if (first_run) {
00080             Umlaufzeit.start();
00081             first_run = false;
00082             return;
00083         }
00084         if (tmp < 1000) return;
00085 
00086         // Cache last 3 values for averaging
00087         Drehzeit[Drehzeit_counter % DREHZEIT_SIZE] = tmp;
00088         ++Drehzeit_counter;
00089         Umlaufzeit.reset();
00090     }
00091 }