Steven Cheldelin
/
eMPC_Simulator_v6
publish
main.cpp@0:910585fe989f, 2020-09-14 (annotated)
- Committer:
- rcflyair
- Date:
- Mon Sep 14 15:01:53 2020 +0000
- Revision:
- 0:910585fe989f
commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
rcflyair | 0:910585fe989f | 1 | #include "mbed.h" |
rcflyair | 0:910585fe989f | 2 | #include "SerialRPCInterface.h" |
rcflyair | 0:910585fe989f | 3 | //#include "MODSERIAL.h" |
rcflyair | 0:910585fe989f | 4 | #define DELAY 250 |
rcflyair | 0:910585fe989f | 5 | |
rcflyair | 0:910585fe989f | 6 | AnalogIn mtr_pos(PA_0); |
rcflyair | 0:910585fe989f | 7 | PwmOut udp_pwm(PA_4); |
rcflyair | 0:910585fe989f | 8 | PwmOut mp_pwm(PB_7); |
rcflyair | 0:910585fe989f | 9 | PwmOut airspeed_pwm(PB_6); |
rcflyair | 0:910585fe989f | 10 | PwmOut rpm_pwm(PA_8); |
rcflyair | 0:910585fe989f | 11 | PwmOut ff_pwm(PA_1); |
rcflyair | 0:910585fe989f | 12 | |
rcflyair | 0:910585fe989f | 13 | DigitalOut myled(LED1); |
rcflyair | 0:910585fe989f | 14 | SerialRPCInterface rpc(PA_9, PA_10, 115200); // create serial rpc interface on usb virtual com port |
rcflyair | 0:910585fe989f | 15 | |
rcflyair | 0:910585fe989f | 16 | float mtr_pos_vout = 0.0f; |
rcflyair | 0:910585fe989f | 17 | float udp_vout = 0.0f; |
rcflyair | 0:910585fe989f | 18 | float mp_vout = 0.0f; |
rcflyair | 0:910585fe989f | 19 | float airspeed_vout = 0.0f; |
rcflyair | 0:910585fe989f | 20 | float ff_f = 0.0f; |
rcflyair | 0:910585fe989f | 21 | float rpm_f = 0.0f; |
rcflyair | 0:910585fe989f | 22 | float temp = 0.25f; |
rcflyair | 0:910585fe989f | 23 | volatile char c = '\0'; // Initialized to the NULL character |
rcflyair | 0:910585fe989f | 24 | |
rcflyair | 0:910585fe989f | 25 | RPCVariable<float> rpc_mtr_pos_vout(&mtr_pos_vout, "mtr_pos_vout"); |
rcflyair | 0:910585fe989f | 26 | RPCVariable<float> rpc_udp_vout(&udp_vout, "udp_vout"); |
rcflyair | 0:910585fe989f | 27 | RPCVariable<float> rpc_mp_vout(&mp_vout, "mp_vout"); |
rcflyair | 0:910585fe989f | 28 | RPCVariable<float> rpc_airspeed_vout(&airspeed_vout, "airspeed_vout"); |
rcflyair | 0:910585fe989f | 29 | RPCVariable<float> rpc_ff_f(&ff_f, "ff_f"); |
rcflyair | 0:910585fe989f | 30 | RPCVariable<float> rpc_temp(&temp, "temp"); |
rcflyair | 0:910585fe989f | 31 | |
rcflyair | 0:910585fe989f | 32 | int pwm_v(float x) |
rcflyair | 0:910585fe989f | 33 | { |
rcflyair | 0:910585fe989f | 34 | return (-4.6206 * x * x * x * x) + (64.395f * x * x * x) - (329.67f * x * x) + (877.84f * x) - 294.19f; |
rcflyair | 0:910585fe989f | 35 | } |
rcflyair | 0:910585fe989f | 36 | |
rcflyair | 0:910585fe989f | 37 | float pwm_ff(float x) |
rcflyair | 0:910585fe989f | 38 | { |
rcflyair | 0:910585fe989f | 39 | return 1/x; |
rcflyair | 0:910585fe989f | 40 | } |
rcflyair | 0:910585fe989f | 41 | |
rcflyair | 0:910585fe989f | 42 | int main() { |
rcflyair | 0:910585fe989f | 43 | |
rcflyair | 0:910585fe989f | 44 | udp_pwm.period_us(1000); |
rcflyair | 0:910585fe989f | 45 | udp_pwm.pulsewidth_us(666); |
rcflyair | 0:910585fe989f | 46 | |
rcflyair | 0:910585fe989f | 47 | mp_pwm.period_us(1000); |
rcflyair | 0:910585fe989f | 48 | mp_pwm.pulsewidth_us(600); |
rcflyair | 0:910585fe989f | 49 | |
rcflyair | 0:910585fe989f | 50 | airspeed_pwm.period_us(1000); |
rcflyair | 0:910585fe989f | 51 | airspeed_pwm.pulsewidth_us(500); |
rcflyair | 0:910585fe989f | 52 | |
rcflyair | 0:910585fe989f | 53 | ff_f = 300.0f; |
rcflyair | 0:910585fe989f | 54 | ff_pwm.period(1/ff_f); |
rcflyair | 0:910585fe989f | 55 | ff_pwm.pulsewidth_us((1/ff_f)/2); |
rcflyair | 0:910585fe989f | 56 | |
rcflyair | 0:910585fe989f | 57 | rpm_f = 400.0f; |
rcflyair | 0:910585fe989f | 58 | rpm_pwm.period(1/rpm_f); |
rcflyair | 0:910585fe989f | 59 | rpm_pwm.pulsewidth((1/rpm_f)/2); |
rcflyair | 0:910585fe989f | 60 | |
rcflyair | 0:910585fe989f | 61 | //rpc.Disable(); |
rcflyair | 0:910585fe989f | 62 | |
rcflyair | 0:910585fe989f | 63 | while(1) { |
rcflyair | 0:910585fe989f | 64 | |
rcflyair | 0:910585fe989f | 65 | myled = !myled; // pulse LED |
rcflyair | 0:910585fe989f | 66 | |
rcflyair | 0:910585fe989f | 67 | mtr_pos_vout = mtr_pos.read(); |
rcflyair | 0:910585fe989f | 68 | |
rcflyair | 0:910585fe989f | 69 | udp_pwm.pulsewidth_us(pwm_v(udp_vout)); |
rcflyair | 0:910585fe989f | 70 | mp_pwm.pulsewidth_us(pwm_v(mp_vout)); |
rcflyair | 0:910585fe989f | 71 | airspeed_pwm.pulsewidth_us(pwm_v(airspeed_vout)); |
rcflyair | 0:910585fe989f | 72 | |
rcflyair | 0:910585fe989f | 73 | rpm_pwm.period(1/rpm_f); |
rcflyair | 0:910585fe989f | 74 | rpm_pwm.pulsewidth((1/rpm_f)/2); |
rcflyair | 0:910585fe989f | 75 | ff_pwm.period(1/ff_f); |
rcflyair | 0:910585fe989f | 76 | ff_pwm.pulsewidth((1/ff_f)/2); |
rcflyair | 0:910585fe989f | 77 | |
rcflyair | 0:910585fe989f | 78 | //rpc.Enable(); |
rcflyair | 0:910585fe989f | 79 | wait_ms(DELAY); |
rcflyair | 0:910585fe989f | 80 | //rpc.Disable(); |
rcflyair | 0:910585fe989f | 81 | } |
rcflyair | 0:910585fe989f | 82 | } |