H2M Teststand / Mbed 2 deprecated H2M_Snippets

Erste Idee für einfache Regelung

Dependencies:   mbed

Fork of Low_Cost_PWM by Hans Dampf

Motorregler_simple.cpp@6:f97371e6bc3e, 2014-09-10 (annotated)

Committer:
Racer01014
Date:
Wed Sep 10 12:59:51 2014 +0000
Revision:
6:f97371e6bc3e
Parent:
5:a68e6c550e4b 
-

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Racer01014 5:a68e6c550e4b 1 #include "mbed.h"
Racer01014 5:a68e6c550e4b 2
Racer01014 5:a68e6c550e4b 3
Racer01014 5:a68e6c550e4b 4 PwmOut Motroregler_PWM(p22);
Racer01014 5:a68e6c550e4b 5
Racer01014 6:f97371e6bc3e 6 volatile int Drehzeit_counter = 0;
Racer01014 6:f97371e6bc3e 7 #define DREHZEIT_SIZE 3
Racer01014 6:f97371e6bc3e 8 volatile int Drehzeit[DREHZEIT_SIZE];
Racer01014 5:a68e6c550e4b 9 int void()
Racer01014 5:a68e6c550e4b 10 {
Racer01014 5:a68e6c550e4b 11
Racer01014 5:a68e6c550e4b 12 //***************************************************************************************************
Racer01014 5:a68e6c550e4b 13 //Control Motor rpm
Racer01014 6:f97371e6bc3e 14
Racer01014 6:f97371e6bc3e 15 int rpm_control(float motor_n_cmd, float motor_n_cur) {
Racer01014 6:f97371e6bc3e 16
Racer01014 6:f97371e6bc3e 17 static int motor_pwm_cmd_last = 900;
Racer01014 6:f97371e6bc3e 18 //static float motor_n_last = 0;
Racer01014 6:f97371e6bc3e 19
Racer01014 6:f97371e6bc3e 20 if (motor_n_cmd < 1.0) {
Racer01014 6:f97371e6bc3e 21 Motroregler_PWM.pulsewidth_us(900);
Racer01014 6:f97371e6bc3e 22 motor_pwm_cmd_last = 900;
Racer01014 5:a68e6c550e4b 23 // motor_n_last = 0;
Racer01014 6:f97371e6bc3e 24 return 1;
Racer01014 6:f97371e6bc3e 25 }
Racer01014 6:f97371e6bc3e 26
Racer01014 6:f97371e6bc3e 27 float motor_n_dif = motor_n_cmd - motor_n_cur;
Racer01014 6:f97371e6bc3e 28
Racer01014 6:f97371e6bc3e 29 int motor_pwm_cmd = (int)(motor_pwm_cmd_last + motor_n_dif * 0.6 + 0.5); // round() ... works only for positive values
Racer01014 6:f97371e6bc3e 30
Racer01014 6:f97371e6bc3e 31 pc.printf("cmd: %7.2f, cur: %7.2f, dif: %7.2f, motor_pwm_cmd: %4d, motor_pwm_dif: %4d, DMS: %f\n\r",
Racer01014 6:f97371e6bc3e 32 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);
Racer01014 6:f97371e6bc3e 33
Racer01014 6:f97371e6bc3e 34 if (motor_pwm_cmd > 1900) motor_pwm_cmd = 1900;
Racer01014 6:f97371e6bc3e 35 else if (motor_pwm_cmd < 1010) motor_pwm_cmd = 1005;
Racer01014 6:f97371e6bc3e 36
Racer01014 6:f97371e6bc3e 37 Motroregler_PWM.pulsewidth_us(motor_pwm_cmd);
Racer01014 6:f97371e6bc3e 38 motor_pwm_cmd_last = motor_pwm_cmd;
Racer01014 6:f97371e6bc3e 39
Racer01014 5:a68e6c550e4b 40 return 1;
Racer01014 5:a68e6c550e4b 41 }
Racer01014 6:f97371e6bc3e 42
Racer01014 5:a68e6c550e4b 43 //***************************************************************************************************
Racer01014 5:a68e6c550e4b 44 //Control MOSFET pwm
Racer01014 6:f97371e6bc3e 45 int brk_mosfet_control(float mosfet_pwm_cmd, float mosfet_pwm_cur) {
Racer01014 6:f97371e6bc3e 46
Racer01014 6:f97371e6bc3e 47 static int mosfet_pwm_cmd_last = 0;
Racer01014 6:f97371e6bc3e 48
Racer01014 6:f97371e6bc3e 49
Racer01014 6:f97371e6bc3e 50 if (mosfet_pwm_cmd < 1.0) {
Racer01014 6:f97371e6bc3e 51 Bremsenstrom_MOSFET.pulsewidth_us(00);
Racer01014 6:f97371e6bc3e 52 mosfet_pwm_cmd_last = 0;
Racer01014 6:f97371e6bc3e 53
Racer01014 6:f97371e6bc3e 54 return 1;
Racer01014 6:f97371e6bc3e 55 }
Racer01014 6:f97371e6bc3e 56
Racer01014 6:f97371e6bc3e 57 float mosfet_pwm_dif = mosfet_pwm_cmd - mosfet_pwm_cur;
Racer01014 6:f97371e6bc3e 58
Racer01014 6:f97371e6bc3e 59 // int mosfet_pwm_cmd = (int)(mosfet_pwm_cmd_last + mosfet_pwm_dif * 0.6 + 0.5); // round() ... works only for positive values
Racer01014 6:f97371e6bc3e 60
Racer01014 6:f97371e6bc3e 61 pc.printf("cmd: %7.2f, cur: %7.2f, dif: %7.2f, mosfet_pwm_cmd: %4d, mosfet_pwm_dif: %4d",
Racer01014 6:f97371e6bc3e 62 mosfet_pwm_cmd, mosfet_pwm_cur, mosfet_pwm_dif, mosfet_pwm_cmd, mosfet_pwm_cmd-mosfet_pwm_cmd_last);
Racer01014 6:f97371e6bc3e 63
Racer01014 6:f97371e6bc3e 64 if (mosfet_pwm_cmd > 20000) mosfet_pwm_cmd = 20000;
Racer01014 6:f97371e6bc3e 65 else if (mosfet_pwm_cmd < 0) mosfet_pwm_cmd = 0;
Racer01014 6:f97371e6bc3e 66
Racer01014 6:f97371e6bc3e 67 Bremsenstrom_MOSFET.pulsewidth_us(mosfet_pwm_cmd);
Racer01014 6:f97371e6bc3e 68
Racer01014 6:f97371e6bc3e 69 mosfet_pwm_cmd_last = mosfet_pwm_cmd;
Racer01014 6:f97371e6bc3e 70
Racer01014 6:f97371e6bc3e 71 return 1;
Racer01014 6:f97371e6bc3e 72 }
Racer01014 6:f97371e6bc3e 73 //***************************************************************************************************
Racer01014 6:f97371e6bc3e 74 //Calculate rpm
Racer01014 6:f97371e6bc3e 75 void Motor_drehzahl() {
Racer01014 6:f97371e6bc3e 76 static bool first_run = true;
Racer01014 6:f97371e6bc3e 77 // Umlaufzeit.stop();
Racer01014 6:f97371e6bc3e 78 int tmp = Umlaufzeit.read_us();
Racer01014 6:f97371e6bc3e 79 if (first_run) {
Racer01014 6:f97371e6bc3e 80 Umlaufzeit.start();
Racer01014 6:f97371e6bc3e 81 first_run = false;
Racer01014 6:f97371e6bc3e 82 return;
Racer01014 6:f97371e6bc3e 83 }
Racer01014 6:f97371e6bc3e 84 if (tmp < 1000) return;
Racer01014 6:f97371e6bc3e 85
Racer01014 6:f97371e6bc3e 86 // Cache last 3 values for averaging
Racer01014 6:f97371e6bc3e 87 Drehzeit[Drehzeit_counter % DREHZEIT_SIZE] = tmp;
Racer01014 6:f97371e6bc3e 88 ++Drehzeit_counter;
Racer01014 6:f97371e6bc3e 89 Umlaufzeit.reset();
Racer01014 6:f97371e6bc3e 90 }
Racer01014 5:a68e6c550e4b 91 }