mirror of
https://github.com/rene-dev/stmbl.git
synced 2024-12-25 01:52:13 +00:00
Compare commits
6 Commits
c37048e667
...
725170347e
Author | SHA1 | Date | |
---|---|---|---|
|
725170347e | ||
|
24f6c6692c | ||
|
5f136f9344 | ||
|
bbf83f2b21 | ||
|
f1ea389f51 | ||
|
f7a8b2d35c |
4
Makefile
4
Makefile
@ -121,8 +121,10 @@ SHARED_COMPS += shared/comps/home.c
|
||||
SHARED_COMPS += shared/comps/en.c
|
||||
SHARED_COMPS += shared/comps/th.c
|
||||
#SHARED_COMPS += shared/comps/asm.c
|
||||
SHARED_COMPS += shared/comps/idp.c
|
||||
SHARED_COMPS += shared/comps/idpmsm.c
|
||||
SHARED_COMPS += shared/comps/iddc.c
|
||||
SHARED_COMPS += shared/comps/idm.c
|
||||
SHARED_COMPS += shared/comps/ids.c
|
||||
#SHARED_COMPS += shared/comps/motsim.c
|
||||
|
||||
COMPS = $(SRC_COMPS) $(SHARED_COMPS)
|
||||
|
@ -29,10 +29,10 @@ conf0.high_dc_volt = 350
|
||||
conf0.low_dc_volt = 24
|
||||
conf0.high_hv_temp = 70
|
||||
conf0.fan_hv_temp = 50
|
||||
conf0.pos_bw = 10
|
||||
conf0.vel_bw = 500
|
||||
conf0.pos_bw = 25
|
||||
conf0.vel_bw = 250
|
||||
conf0.vel_d = 5
|
||||
conf0.vel_g = 0.1
|
||||
conf0.torque_g = 1
|
||||
conf0.cur_bw = 1500
|
||||
conf0.cur_bw = 1000
|
||||
conf0.max_sat = 0.2
|
||||
|
@ -1,18 +1,7 @@
|
||||
load dc_limits
|
||||
load dc_ttc
|
||||
dc_limits0.rt_prio = 7
|
||||
dc_ttc0.rt_prio = 9
|
||||
dc_limits0.r = conf0.r
|
||||
dc_limits0.ld = conf0.l
|
||||
dc_limits0.lq = conf0.l
|
||||
dc_limits0.psi = conf0.psi
|
||||
dc_ttc0.psi = conf0.psi
|
||||
dc_limits0.ac_volt = hv0.pwm_volt
|
||||
dc_limits0.iq = hv0.iq_fb
|
||||
pid0.max_torque = dc_limits0.max_torque
|
||||
pid0.min_torque = dc_limits0.min_torque
|
||||
pid0.max_vel = dc_limits0.abs_max_vel
|
||||
dc_ttc0.torque = pid0.torque_cor_cmd
|
||||
dc_ttc0.torque = pid0.torque_cmd
|
||||
hv0.d_cmd = 0
|
||||
hv0.pos = 0
|
||||
hv0.phase_mode = 3
|
||||
|
28
conf/template/iddc.txt
Normal file
28
conf/template/iddc.txt
Normal file
@ -0,0 +1,28 @@
|
||||
|
||||
load iddc
|
||||
iddc0.rt_prio = 4
|
||||
|
||||
iddc0.en = fault0.en_out
|
||||
fault0.pos_error = 0
|
||||
pid0.enable = 0
|
||||
hv0.en = iddc0.en_out
|
||||
hv0.cur_bw = iddc0.cur_bw
|
||||
hv0.cmd_mode = iddc0.cmd_mode
|
||||
hv0.d_cmd = iddc0.d_cmd
|
||||
hv0.q_cmd = iddc0.q_cmd
|
||||
hv0.pos = iddc0.com_pos
|
||||
hv0.rev = iddc0.out_rev
|
||||
hv0.r = idq0.r
|
||||
hv0.l = idq0.l
|
||||
hv0.phase_mode = 3
|
||||
iddc0.ud_fb = hv0.ud_fb
|
||||
iddc0.uq_fb = hv0.uq_fb
|
||||
iddc0.id_fb = hv0.id_fb
|
||||
iddc0.iq_fb = hv0.iq_fb
|
||||
iddc0.pwm_volt = hv0.pwm_volt
|
||||
|
||||
iddc0.pos_fb = fb_switch0.pos_fb
|
||||
iddc0.vel_fb = vel1.vel
|
||||
|
||||
vel0.pos_in = fb_switch0.pos_fb
|
||||
iddc0.vel_fb = vel1.vel
|
@ -1,27 +0,0 @@
|
||||
|
||||
load idp
|
||||
idp0.rt_prio = 4
|
||||
|
||||
idp0.en = fault0.en_out
|
||||
fault0.pos_error = 0
|
||||
pid0.enable = 0
|
||||
hv0.en = idp0.en_out
|
||||
hv0.cur_bw = idp0.cur_bw
|
||||
hv0.cmd_mode = idp0.cmd_mode
|
||||
hv0.d_cmd = idp0.d_cmd
|
||||
hv0.q_cmd = idp0.q_cmd
|
||||
hv0.pos = idp0.com_pos
|
||||
hv0.rev = idp0.out_rev
|
||||
hv0.r = idq0.r
|
||||
hv0.l = idq0.l
|
||||
idp0.ud_fb = hv0.ud_fb
|
||||
idp0.uq_fb = hv0.uq_fb
|
||||
idp0.id_fb = hv0.id_fb
|
||||
idp0.iq_fb = hv0.iq_fb
|
||||
idp0.pwm_volt = hv0.pwm_volt
|
||||
|
||||
idp0.pos_fb = fb_switch0.pos_fb
|
||||
idp0.vel_fb = vel1.vel
|
||||
|
||||
vel0.pos_in = fb_switch0.pos_fb
|
||||
idp0.vel_fb = vel1.vel
|
28
conf/template/idpmsm.txt
Normal file
28
conf/template/idpmsm.txt
Normal file
@ -0,0 +1,28 @@
|
||||
|
||||
load idpmsm
|
||||
idpmsm0.rt_prio = 4
|
||||
|
||||
idpmsm0.en = fault0.en_out
|
||||
fault0.pos_error = 0
|
||||
fault0.sat_error = 0
|
||||
pid0.en = 0
|
||||
hv0.en = idpmsm0.en_out
|
||||
hv0.cur_bw = idpmsm0.cur_bw
|
||||
hv0.cmd_mode = idpmsm0.cmd_mode
|
||||
hv0.d_cmd = idpmsm0.d_cmd
|
||||
hv0.q_cmd = idpmsm0.q_cmd
|
||||
hv0.pos = idpmsm0.com_pos
|
||||
hv0.rev = idpmsm0.out_rev
|
||||
hv0.r = idq0.r
|
||||
hv0.l = idq0.l
|
||||
idpmsm0.ud_fb = hv0.ud_fb
|
||||
idpmsm0.uq_fb = hv0.uq_fb
|
||||
idpmsm0.id_fb = hv0.id_fb
|
||||
idpmsm0.iq_fb = hv0.iq_fb
|
||||
idpmsm0.pwm_volt = hv0.pwm_volt
|
||||
|
||||
idpmsm0.pos_fb = fb_switch0.mot_abs_fb_no_offset
|
||||
idpmsm0.vel_fb = vel1.vel
|
||||
|
||||
vel0.pos_in = fb_switch0.pos_fb
|
||||
idpmsm0.vel_fb = vel1.vel
|
@ -1,9 +1,37 @@
|
||||
|
||||
link idm
|
||||
load ids
|
||||
ids0.rt_prio = 4
|
||||
|
||||
idm0.sys = 1
|
||||
pid0.j_mot = conf0.j
|
||||
pid0.j_sys = idm0.inertia
|
||||
pid0.d = idm0.damping
|
||||
pid0.f = idm0.friction
|
||||
pid0.l = idm0.load
|
||||
ids0.en = fault0.en_out
|
||||
pid0.pos_ext_cmd = ids0.pos_cmd
|
||||
pid0.vel_ext_cmd = ids0.vel_cmd
|
||||
pid0.acc_ext_cmd = ids0.acc_cmd
|
||||
|
||||
pid0.pos_bw = ids0.pos_bw
|
||||
pid0.vel_bw = ids0.vel_bw
|
||||
pid0.vel_d = ids0.vel_d
|
||||
|
||||
ids0.pos_error = pid0.pos_error
|
||||
ids0.vel_error = pid0.vel_error
|
||||
|
||||
ids0.cur_bw = conf0.cur_bw
|
||||
|
||||
conf0.max_pos_error = 0
|
||||
conf0.max_sat = 10
|
||||
conf0.vel_g = 1
|
||||
|
||||
term0.wave0 = ids0.pos_cmd
|
||||
term0.wave2 = ids0.vel_cmd
|
||||
term0.wave4 = ids0.min_cost
|
||||
term0.gain4 = 20
|
||||
#term0.wave1 = ids0.cost
|
||||
#term0.gain1 = 20
|
||||
|
||||
term0.wave5 = ids0.pos_bw
|
||||
term0.gain5 = 1.0;
|
||||
term0.wave6 = ids0.vel_bw
|
||||
term0.gain6 = 0.1
|
||||
term0.wave7 = ids0.vel_d
|
||||
term0.gain7 = 10.0
|
||||
|
||||
term0.send_step = 21
|
2
docs/src/cmd.adoc
Normal file
2
docs/src/cmd.adoc
Normal file
@ -0,0 +1,2 @@
|
||||
:lang: en
|
||||
|
36
docs/src/feedback.adoc
Normal file
36
docs/src/feedback.adoc
Normal file
@ -0,0 +1,36 @@
|
||||
:lang: en
|
||||
|
||||
= Feedback
|
||||
== incremental Encoder
|
||||
=== pinout
|
||||
=== config
|
||||
[source]
|
||||
link enc_fb0
|
||||
conf0.mot_fb_res = 4096
|
||||
|
||||
== sin/cos Encoder
|
||||
=== pinout
|
||||
=== config
|
||||
[source]
|
||||
link enc_fb0
|
||||
conf0.mot_fb_res = 4096
|
||||
fb_switch0.mot_pos = enc_fb0.ipos
|
||||
|
||||
|
||||
== Resolver
|
||||
=== pinout
|
||||
=== config
|
||||
[source]
|
||||
link res_fb0
|
||||
|
||||
== Tamagawa smartabs
|
||||
=== pinout
|
||||
=== config
|
||||
[source]
|
||||
link smartabs_fb0
|
||||
|
||||
== Mitsubishi
|
||||
=== pinout
|
||||
=== config
|
||||
[source]
|
||||
link encm_fb0
|
18
docs/src/motor.adoc
Normal file
18
docs/src/motor.adoc
Normal file
@ -0,0 +1,18 @@
|
||||
:lang: en
|
||||
|
||||
= Motor
|
||||
== DC
|
||||
=== pinout
|
||||
=== config
|
||||
[source]
|
||||
link dc
|
||||
|
||||
== PMSM
|
||||
=== pinout
|
||||
=== config
|
||||
[source]
|
||||
link pmsm
|
||||
|
||||
== ASYNC
|
||||
=== pinout
|
||||
=== config
|
2
docs/src/pinout.adoc
Normal file
2
docs/src/pinout.adoc
Normal file
@ -0,0 +1,2 @@
|
||||
:lang: en
|
||||
|
3
docs/src/servoterm.adoc
Normal file
3
docs/src/servoterm.adoc
Normal file
@ -0,0 +1,3 @@
|
||||
:lang: en
|
||||
|
||||
= servoterm
|
8
docs/src/supply.adoc
Normal file
8
docs/src/supply.adoc
Normal file
@ -0,0 +1,8 @@
|
||||
:lang: en
|
||||
|
||||
= Supply
|
||||
|
||||
== LV supply
|
||||
24V 0.5A
|
||||
|
||||
== HV supply
|
155
docs/src/tuning.adoc
Normal file
155
docs/src/tuning.adoc
Normal file
@ -0,0 +1,155 @@
|
||||
:lang: en
|
||||
|
||||
= Parameter tuning
|
||||
|
||||
== Requirements
|
||||
- link:supply.adoc[supply]
|
||||
- link:servoterm.adoc[servoterm]
|
||||
- link:feedback.adoc[feedback]
|
||||
- link:motor.adoc[motor]
|
||||
|
||||
== electrical parameters
|
||||
disconnect the motor from the load
|
||||
|
||||
=== dc motor
|
||||
==== manual tuning
|
||||
[source]
|
||||
conf0.r // motor phase resistance
|
||||
conf0.l // motor phase inductance
|
||||
conf0.psi // torque constant [Nm/A] or [V/rad/s]
|
||||
|
||||
==== auto tuning
|
||||
this takes about 20 seconds
|
||||
the motor will move
|
||||
follow the instructions in Servoterm
|
||||
|
||||
config:
|
||||
[source]
|
||||
link pid
|
||||
link dc
|
||||
link <your_feedback_type>
|
||||
link misc
|
||||
link iddc
|
||||
|
||||
start:
|
||||
[source]
|
||||
fault0.en = 1
|
||||
|
||||
example output:
|
||||
[source]
|
||||
OK fault0.en = 1.000000
|
||||
INFO: Enabled
|
||||
Measure r, l
|
||||
block the rotor
|
||||
id0.state = 1.2 to start
|
||||
OK id0.state = 1.200000
|
||||
conf0.r = 1.898350
|
||||
conf0.l = 0.013483
|
||||
Measure torque constant
|
||||
unblock the rotor
|
||||
the motor will move
|
||||
id0.state = 2.2 to start
|
||||
OK id0.state = 2.200000
|
||||
conf0.psi = 0.063680
|
||||
|
||||
=== pmsm motor
|
||||
==== manual tuning
|
||||
[source]
|
||||
conf0.r // motor phase resistance
|
||||
conf0.l // motor phase inductance
|
||||
conf0.polecount // number of pole pairs
|
||||
conf0.mot_fb_offset
|
||||
conf0.mot_fb_rev
|
||||
conf0.psi // torque constant [Nm/A] or [V/rad/s]
|
||||
|
||||
==== auto tuning
|
||||
this takes about 20 seconds
|
||||
the motor will move
|
||||
|
||||
config:
|
||||
[source]
|
||||
link pid
|
||||
link pmsm
|
||||
link <your_feedback_type>
|
||||
link misc
|
||||
link idpmsm
|
||||
|
||||
start:
|
||||
[source]
|
||||
fault0.en = 1
|
||||
|
||||
example output:
|
||||
[source]
|
||||
OK fault0.en = 1.000000
|
||||
INFO: Enabled
|
||||
conf0.r = 1.898350
|
||||
conf0.l = 0.013483
|
||||
conf0.polecount = 4.000000
|
||||
conf0.mot_fb_offset = -0.258811
|
||||
conf0.psi = 0.063680
|
||||
|
||||
== mechanical parameters
|
||||
connect the motor to the load
|
||||
|
||||
=== manual tuning
|
||||
[source]
|
||||
conf0.j
|
||||
conf0.j_load
|
||||
|
||||
=== auto tuning
|
||||
this takes about 3 minutes
|
||||
the motor will move
|
||||
|
||||
config:
|
||||
[source]
|
||||
link pid
|
||||
link <your_motor_type>
|
||||
link <your_feedback_type>
|
||||
link misc
|
||||
link idm
|
||||
<your_electrical_parameters>
|
||||
|
||||
start:
|
||||
[source]
|
||||
fault0.en = 1
|
||||
|
||||
example output:
|
||||
[source]
|
||||
OK fault0.en = 1.000000
|
||||
INFO: Enabled
|
||||
conf0.j = 0.000085
|
||||
conf0.d = 0.000022
|
||||
conf0.f = 0.057124
|
||||
|
||||
== control loop tuning
|
||||
=== manual tuning
|
||||
[source]
|
||||
conf0.pos_bw
|
||||
conf0.vel_bw
|
||||
conf0.vel_d
|
||||
|
||||
=== auto tuning
|
||||
this takes about 2 minutes
|
||||
the motor will move
|
||||
|
||||
config:
|
||||
[source]
|
||||
link pid
|
||||
link <your_motor_type>
|
||||
link <your_feedback_type>
|
||||
link misc
|
||||
link ids
|
||||
<your_electrical_parameters>
|
||||
<your_mechanical_parameters>
|
||||
|
||||
start:
|
||||
[source]
|
||||
fault0.en = 1
|
||||
|
||||
example output:
|
||||
[source]
|
||||
OK fault0.en = 1.000000
|
||||
INFO: Enabled
|
||||
conf0.pos_bw = 24.165899
|
||||
conf0.vel_bw = 450.000000
|
||||
conf0.vel_d = 6.058537
|
@ -20,7 +20,7 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
// struct dc_ttc_ctx_t * ctx = (struct dc_ttc_ctx_t *)ctx_ptr;
|
||||
struct dc_ttc_pin_ctx_t *pins = (struct dc_ttc_pin_ctx_t *)pin_ptr;
|
||||
|
||||
float psi_m = MAX(PIN(psi), 0.01);
|
||||
float psi_m = MAX(PIN(psi), 0.001);
|
||||
float torque = PIN(torque);
|
||||
|
||||
PIN(cur) = torque / psi_m;
|
||||
|
@ -96,6 +96,19 @@ struct fault_ctx_t {
|
||||
float mot_temp_error;
|
||||
};
|
||||
|
||||
void enable(char *ptr) {
|
||||
hal_parse("fault0.en = 1");
|
||||
}
|
||||
|
||||
COMMAND("enable", enable, "enable");
|
||||
|
||||
void disable(char *ptr) {
|
||||
hal_parse("fault0.en = 0");
|
||||
}
|
||||
|
||||
COMMAND("disable", disable, "disable");
|
||||
|
||||
|
||||
static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
struct fault_ctx_t *ctx = (struct fault_ctx_t *)ctx_ptr;
|
||||
struct fault_pin_ctx_t *pins = (struct fault_pin_ctx_t *)pin_ptr;
|
||||
|
245
shared/comps/iddc.c
Normal file
245
shared/comps/iddc.c
Normal file
@ -0,0 +1,245 @@
|
||||
#include "iddc_comp.h"
|
||||
#include "hal.h"
|
||||
#include "defines.h"
|
||||
#include "angle.h"
|
||||
|
||||
HAL_COMP(iddc);
|
||||
|
||||
HAL_PIN(d_cmd);
|
||||
HAL_PIN(q_cmd);
|
||||
HAL_PIN(com_pos);
|
||||
HAL_PIN(cmd_mode);
|
||||
HAL_PIN(en);
|
||||
HAL_PIN(en_out);
|
||||
|
||||
HAL_PIN(id_fb);
|
||||
HAL_PIN(iq_fb);
|
||||
HAL_PIN(ud_fb);
|
||||
HAL_PIN(uq_fb);
|
||||
HAL_PIN(pos_fb);
|
||||
HAL_PIN(vel_fb);
|
||||
|
||||
HAL_PIN(state);
|
||||
HAL_PIN(timer);
|
||||
|
||||
HAL_PIN(r);
|
||||
HAL_PIN(l);
|
||||
|
||||
HAL_PIN(pp);
|
||||
HAL_PIN(com_offset);
|
||||
HAL_PIN(out_rev);
|
||||
|
||||
HAL_PIN(test_cur);
|
||||
HAL_PIN(test_vel);
|
||||
HAL_PIN(ki);
|
||||
HAL_PIN(vel_bw);
|
||||
|
||||
HAL_PIN(pi);
|
||||
|
||||
HAL_PIN(pwm_volt);
|
||||
|
||||
HAL_PIN(psi);
|
||||
|
||||
HAL_PIN(cur_bw);
|
||||
HAL_PIN(cur_sum);
|
||||
|
||||
HAL_PIN(tmp0);
|
||||
HAL_PIN(tmp1);
|
||||
HAL_PIN(tmp2);
|
||||
HAL_PIN(tmp3);
|
||||
HAL_PIN(avg_test_volt);
|
||||
|
||||
static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
//struct iddc_ctx_t * ctx = (struct iddc_ctx_t *)ctx_ptr;
|
||||
struct iddc_pin_ctx_t *pins = (struct iddc_pin_ctx_t *)pin_ptr;
|
||||
PIN(test_cur) = 4.0;
|
||||
PIN(test_vel) = 50.0;
|
||||
PIN(ki) = 1.0;
|
||||
PIN(pi) = 1.0;
|
||||
PIN(vel_bw) = 20.0;
|
||||
PIN(cur_bw) = 1.0;
|
||||
}
|
||||
|
||||
static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
//struct iddc_ctx_t * ctx = (struct iddc_ctx_t *)ctx_ptr;
|
||||
struct iddc_pin_ctx_t *pins = (struct iddc_pin_ctx_t *)pin_ptr;
|
||||
|
||||
switch((int)(PIN(state) * 10.0 + 0.5)){
|
||||
case 0:
|
||||
PIN(r) = 0.1;
|
||||
PIN(l) = 0.001;
|
||||
PIN(psi) = 0.055;
|
||||
PIN(pp) = 3.0;
|
||||
PIN(com_offset) = 0.0;
|
||||
PIN(out_rev) = 0.0;
|
||||
PIN(cur_bw) = 1.0;
|
||||
break;
|
||||
|
||||
case 10: // r
|
||||
PIN(state) = 1.1;
|
||||
PIN(timer) = 0.0;
|
||||
PIN(d_cmd) = 0.0;
|
||||
PIN(q_cmd) = 0.0;
|
||||
PIN(com_pos) = 0.0;
|
||||
PIN(cmd_mode) = 0.0;
|
||||
|
||||
printf("Measure r, l\n");
|
||||
printf("block the rotor\n");
|
||||
printf("id0.state = 1.2 to start\n");
|
||||
break;
|
||||
|
||||
case 14:
|
||||
printf("conf0.r = %f # append to config\n", PIN(r));
|
||||
printf("conf0.l = %f # append to config\n", PIN(l));
|
||||
PIN(state) = 2.0;
|
||||
break;
|
||||
|
||||
case 20:
|
||||
PIN(state) = 3.1;
|
||||
PIN(timer) = 0.0;
|
||||
PIN(d_cmd) = 0.0;
|
||||
PIN(q_cmd) = 0.0;
|
||||
PIN(cur_sum) = 0.0;
|
||||
PIN(cmd_mode) = 0.0;
|
||||
PIN(cur_bw) = 250.0;
|
||||
|
||||
printf("Measure torque constant\n");
|
||||
printf("unblock the rotor\n");
|
||||
printf("the motor will move\n");
|
||||
printf("id0.state = 2.2 to start\n");
|
||||
break;
|
||||
|
||||
case 23:
|
||||
printf("conf0.psi = %f # append to config\n", PIN(psi));
|
||||
printf("done\n");
|
||||
|
||||
PIN(state) = 2.4;
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
//struct iddc_ctx_t * ctx = (struct iddc_ctx_t *)ctx_ptr;
|
||||
struct iddc_pin_ctx_t *pins = (struct iddc_pin_ctx_t *)pin_ptr;
|
||||
|
||||
if(PIN(en) <= 0.0){
|
||||
PIN(state) = 0.0;
|
||||
}
|
||||
|
||||
switch((int)(PIN(state) * 10.0 + 0.5)){
|
||||
case 0:
|
||||
PIN(en_out) = 0.0;
|
||||
PIN(cmd_mode) = 0.0;
|
||||
PIN(timer) = 0.0;
|
||||
PIN(d_cmd) = 0.0;
|
||||
PIN(q_cmd) = 0.0;
|
||||
PIN(cur_bw) = 1.0;
|
||||
|
||||
if(PIN(en) > 0.0){
|
||||
PIN(state) = 1.0;
|
||||
}
|
||||
break;
|
||||
|
||||
case 12: // r
|
||||
PIN(en_out) = 1.0;
|
||||
PIN(cmd_mode) = 1.0;
|
||||
PIN(cur_bw) = 1.0;
|
||||
PIN(q_cmd) = 0.0;
|
||||
PIN(com_pos) = 0.0;
|
||||
|
||||
PIN(d_cmd) = PIN(test_cur);
|
||||
|
||||
PIN(r) = PIN(r) * 0.99 + PIN(ud_fb) / MAX(PIN(id_fb), 0.01) * 0.01;
|
||||
|
||||
PIN(timer) += period;
|
||||
if(PIN(timer) >= 2.0){
|
||||
PIN(timer) = 0.0;
|
||||
PIN(state) = 1.3;
|
||||
PIN(d_cmd) = 0.0;
|
||||
PIN(en_out) = 0.0;
|
||||
PIN(tmp0) = 0.0;
|
||||
PIN(tmp1) = 0.0;
|
||||
|
||||
PIN(avg_test_volt) = PIN(test_cur) * MAX(PIN(r), 0.01);
|
||||
PIN(avg_test_volt) = LIMIT(PIN(avg_test_volt), PIN(pwm_volt) / 2.0);
|
||||
}
|
||||
break;
|
||||
|
||||
case 13: // l
|
||||
PIN(en_out) = 1.0;
|
||||
PIN(cmd_mode) = 0.0;
|
||||
PIN(q_cmd) = 0.0;
|
||||
PIN(cur_bw) = 1.0;
|
||||
|
||||
//PIN(l) = PIN(l) * 0.995 + ABS(PIN(ud_fb) - avg_test_volt / 2.0) / MAX(ABS(PIN(id_fb) - PIN(test_cur)), 0.001) * period * 0.005;
|
||||
if(PIN(d_cmd) < PIN(avg_test_volt)){
|
||||
PIN(tmp0) = PIN(tmp0) * 0.99 + PIN(id_fb) * 0.01;
|
||||
PIN(tmp1) = PIN(tmp1) * 0.99 + PIN(ud_fb) * 0.01;
|
||||
PIN(d_cmd) = PIN(avg_test_volt) * 1.5;
|
||||
}
|
||||
else{
|
||||
PIN(tmp2) = PIN(tmp2) * 0.99 + PIN(id_fb) * 0.01;
|
||||
PIN(tmp3) = PIN(tmp3) * 0.99 + PIN(ud_fb) * 0.01;
|
||||
PIN(d_cmd) = PIN(avg_test_volt) * 0.5;
|
||||
}
|
||||
|
||||
PIN(timer) += period;
|
||||
if(PIN(timer) >= 1.0){
|
||||
PIN(l) = ABS(PIN(tmp1) - PIN(tmp3)) / ABS(PIN(tmp0) - PIN(tmp2)) * period;
|
||||
PIN(timer) = 0.0;
|
||||
PIN(state) = 1.4;
|
||||
PIN(d_cmd) = PIN(avg_test_volt);
|
||||
PIN(en_out) = 0.0;
|
||||
// PIN(tmp0) = 0.0;
|
||||
// PIN(tmp1) = 0.0;
|
||||
}
|
||||
break;
|
||||
|
||||
case 22: // psi
|
||||
PIN(en_out) = 1.0;
|
||||
PIN(cmd_mode) = 1.0;
|
||||
PIN(cur_bw) = 250.0;
|
||||
PIN(d_cmd) = 0.0;
|
||||
|
||||
float vel_error = PIN(test_vel) - PIN(vel_fb);
|
||||
PIN(cur_sum) += PIN(ki) * vel_error * period;
|
||||
|
||||
PIN(q_cmd) = PIN(vel_bw) * period * vel_error + PIN(cur_sum);
|
||||
|
||||
if(ABS(PIN(vel_fb)) > 0.1){
|
||||
float psi = (PIN(uq_fb) - PIN(iq_fb) * PIN(r)) / (PIN(vel_fb) * PIN(pp));
|
||||
PIN(psi) = PIN(psi) * (1.0 - period / PIN(pi)) + psi * period / PIN(pi);
|
||||
}
|
||||
|
||||
PIN(psi) = CLAMP(PIN(psi), 0.001, 1.0);
|
||||
|
||||
PIN(timer) += period;
|
||||
if(PIN(timer) >= 5.0){
|
||||
PIN(timer) = 0.0;
|
||||
PIN(en_out) = 0.0;
|
||||
PIN(d_cmd) = 0.0;
|
||||
PIN(q_cmd) = 0.0;
|
||||
PIN(cur_sum) = 0.0;
|
||||
PIN(cmd_mode) = 0.0;
|
||||
|
||||
PIN(state) = 2.3;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
hal_comp_t iddc_comp_struct = {
|
||||
.name = "iddc",
|
||||
.nrt = nrt,
|
||||
.rt = rt_func,
|
||||
.frt = 0,
|
||||
.nrt_init = nrt_init,
|
||||
.rt_start = 0,
|
||||
.frt_start = 0,
|
||||
.rt_stop = 0,
|
||||
.frt_stop = 0,
|
||||
.ctx_size = 0,//sizeof(struct iddc_ctx_t),
|
||||
.pin_count = sizeof(struct iddc_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
|
||||
};
|
@ -90,18 +90,18 @@ static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
|
||||
case 13:
|
||||
if(PIN(sys) > 0.0){
|
||||
printf("conf0.j_sys = %f\n", PIN(inertia));
|
||||
printf("conf0.j_sys = %f # append to config\n", PIN(inertia));
|
||||
}
|
||||
else{
|
||||
printf("conf0.j = %f\n", PIN(inertia));
|
||||
printf("conf0.j = %f # append to config\n", PIN(inertia));
|
||||
}
|
||||
printf("conf0.d = %f\n", PIN(damping));
|
||||
printf("conf0.f = %f\n", PIN(friction));
|
||||
printf("pid0.l = %f\n", PIN(load));
|
||||
printf("conf0.d = %f # append to config\n", PIN(damping));
|
||||
printf("conf0.f = %f # append to config\n", PIN(friction));
|
||||
printf("done\n");
|
||||
//printf("pid0.l = %f\n", PIN(load));
|
||||
PIN(state) = 1.4;
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
|
@ -1,9 +1,9 @@
|
||||
#include "idp_comp.h"
|
||||
#include "idpmsm_comp.h"
|
||||
#include "hal.h"
|
||||
#include "defines.h"
|
||||
#include "angle.h"
|
||||
|
||||
HAL_COMP(idp);
|
||||
HAL_COMP(idpmsm);
|
||||
|
||||
HAL_PIN(d_cmd);
|
||||
HAL_PIN(q_cmd);
|
||||
@ -51,8 +51,8 @@ HAL_PIN(tmp3);
|
||||
HAL_PIN(avg_test_volt);
|
||||
|
||||
static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
//struct idp_ctx_t * ctx = (struct idp_ctx_t *)ctx_ptr;
|
||||
struct idp_pin_ctx_t *pins = (struct idp_pin_ctx_t *)pin_ptr;
|
||||
//struct idpmsm_ctx_t * ctx = (struct idpmsm_ctx_t *)ctx_ptr;
|
||||
struct idpmsm_pin_ctx_t *pins = (struct idpmsm_pin_ctx_t *)pin_ptr;
|
||||
PIN(test_cur) = 4.0;
|
||||
PIN(test_vel) = 50.0;
|
||||
PIN(ki) = 1.0;
|
||||
@ -63,8 +63,8 @@ static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
}
|
||||
|
||||
static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
//struct idp_ctx_t * ctx = (struct idp_ctx_t *)ctx_ptr;
|
||||
struct idp_pin_ctx_t *pins = (struct idp_pin_ctx_t *)pin_ptr;
|
||||
//struct idpmsm_ctx_t * ctx = (struct idpmsm_ctx_t *)ctx_ptr;
|
||||
struct idpmsm_pin_ctx_t *pins = (struct idpmsm_pin_ctx_t *)pin_ptr;
|
||||
|
||||
switch((int)(PIN(state) * 10.0 + 0.5)){
|
||||
case 0:
|
||||
@ -96,8 +96,8 @@ static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
break;
|
||||
|
||||
case 14:
|
||||
printf("conf0.r = %f\n", PIN(r));
|
||||
printf("conf0.l = %f\n", PIN(l));
|
||||
printf("conf0.r = %f # append to config\n", PIN(r));
|
||||
printf("conf0.l = %f # append to config\n", PIN(l));
|
||||
PIN(state) = 2.0;
|
||||
break;
|
||||
|
||||
@ -120,10 +120,10 @@ static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
break;
|
||||
|
||||
case 25: // pp, out_rev, com_offset
|
||||
printf("conf0.polecount = %f\n", PIN(pp));
|
||||
printf("conf0.mot_fb_offset = %f\n", PIN(com_offset));
|
||||
printf("conf0.polecount = %f # append to config\n", PIN(pp));
|
||||
printf("conf0.mot_fb_offset = %f # append to config\n", PIN(com_offset));
|
||||
if(PIN(out_rev) > 0.0){
|
||||
printf("conf0.out_rev = 1\n");
|
||||
printf("conf0.out_rev = 1 # append to config\n");
|
||||
}
|
||||
PIN(state) = 3.0;
|
||||
break;
|
||||
@ -148,7 +148,8 @@ static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
break;
|
||||
|
||||
case 33:
|
||||
printf("conf0.psi = %f\n", PIN(psi));
|
||||
printf("conf0.psi = %f # append to config\n", PIN(psi));
|
||||
printf("done\n");
|
||||
|
||||
PIN(state) = 3.4;
|
||||
break;
|
||||
@ -157,8 +158,8 @@ static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
}
|
||||
|
||||
static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
//struct idp_ctx_t * ctx = (struct idp_ctx_t *)ctx_ptr;
|
||||
struct idp_pin_ctx_t *pins = (struct idp_pin_ctx_t *)pin_ptr;
|
||||
//struct idpmsm_ctx_t * ctx = (struct idpmsm_ctx_t *)ctx_ptr;
|
||||
struct idpmsm_pin_ctx_t *pins = (struct idpmsm_pin_ctx_t *)pin_ptr;
|
||||
|
||||
if(PIN(en) <= 0.0){
|
||||
PIN(state) = 0.0;
|
||||
@ -272,7 +273,7 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
PIN(d_cmd) = LIMIT(PIN(d_cmd), PIN(pwm_volt));
|
||||
PIN(com_pos) = 0.0;
|
||||
|
||||
PIN(com_offset) = PIN(com_offset) * 0.99 + -PIN(pos_fb) * 0.01;
|
||||
PIN(com_offset) = PIN(com_offset) * 0.95 - PIN(pos_fb) * 0.05;
|
||||
|
||||
PIN(timer) += period;
|
||||
if(PIN(timer) >= 2.0){
|
||||
@ -321,8 +322,8 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
}
|
||||
|
||||
|
||||
hal_comp_t idp_comp_struct = {
|
||||
.name = "idp",
|
||||
hal_comp_t idpmsm_comp_struct = {
|
||||
.name = "idpmsm",
|
||||
.nrt = nrt,
|
||||
.rt = rt_func,
|
||||
.frt = 0,
|
||||
@ -331,6 +332,6 @@ hal_comp_t idp_comp_struct = {
|
||||
.frt_start = 0,
|
||||
.rt_stop = 0,
|
||||
.frt_stop = 0,
|
||||
.ctx_size = 0,//sizeof(struct idp_ctx_t),
|
||||
.pin_count = sizeof(struct idp_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
|
||||
.ctx_size = 0,//sizeof(struct idpmsm_ctx_t),
|
||||
.pin_count = sizeof(struct idpmsm_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
|
||||
};
|
245
shared/comps/ids.c
Normal file
245
shared/comps/ids.c
Normal file
@ -0,0 +1,245 @@
|
||||
#include "ids_comp.h"
|
||||
#include "hal.h"
|
||||
#include "defines.h"
|
||||
#include "angle.h"
|
||||
|
||||
HAL_COMP(ids);
|
||||
|
||||
HAL_PIN(en);
|
||||
|
||||
HAL_PIN(state);
|
||||
HAL_PIN(param);
|
||||
HAL_PIN(step);
|
||||
HAL_PIN(rep);
|
||||
|
||||
HAL_PIN(freq);
|
||||
HAL_PIN(amp);
|
||||
HAL_PIN(min_pos);
|
||||
HAL_PIN(max_pos);
|
||||
HAL_PIN(max_vel);
|
||||
HAL_PIN(max_acc);
|
||||
|
||||
HAL_PIN(pos);
|
||||
HAL_PIN(vel);
|
||||
HAL_PIN(acc);
|
||||
HAL_PIN(pos_cmd);
|
||||
HAL_PIN(vel_cmd);
|
||||
HAL_PIN(acc_cmd);
|
||||
|
||||
HAL_PIN(pos_error);
|
||||
HAL_PIN(vel_error);
|
||||
|
||||
HAL_PIN(pos_bw);
|
||||
HAL_PIN(vel_bw);
|
||||
HAL_PIN(vel_d);
|
||||
HAL_PIN(cur_bw);
|
||||
|
||||
HAL_PIN(ff);
|
||||
HAL_PIN(kp);
|
||||
HAL_PIN(ks);
|
||||
HAL_PIN(kv);
|
||||
HAL_PIN(kt);
|
||||
HAL_PIN(kd);
|
||||
|
||||
HAL_PINA(params, 3);
|
||||
HAL_PINA(max_params, 3);
|
||||
|
||||
HAL_PIN(target);
|
||||
HAL_PIN(cost);
|
||||
HAL_PIN(min_cost);
|
||||
HAL_PIN(auto_step);
|
||||
|
||||
HAL_PIN(timer);
|
||||
|
||||
static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
//struct ids_ctx_t * ctx = (struct ids_ctx_t *)ctx_ptr;
|
||||
struct ids_pin_ctx_t *pins = (struct ids_pin_ctx_t *)pin_ptr;
|
||||
PIN(pos_bw) = 10.0;
|
||||
PIN(vel_bw) = 100.0;
|
||||
PIN(vel_d) = 10.0;
|
||||
PINA(params, 0) = PIN(vel_bw);
|
||||
PINA(params, 1) = 1.0 / PIN(vel_d);
|
||||
PINA(params, 2) = PIN(pos_bw);
|
||||
|
||||
PIN(kp) = 1.0;
|
||||
PIN(ks) = 0.0;
|
||||
PIN(kv) = 1.0;
|
||||
PIN(kt) = 1.2;
|
||||
PIN(kd) = 0.7;
|
||||
|
||||
PIN(ff) = 1.0;
|
||||
|
||||
PIN(max_vel) = 100.0;
|
||||
PIN(max_acc) = 1000.0;
|
||||
PIN(min_pos) = -10.0;
|
||||
PIN(max_pos) = 10.0;
|
||||
|
||||
PIN(param) = 0.0;
|
||||
PIN(step) = 0.25;
|
||||
PIN(rep) = 2.0;
|
||||
|
||||
PIN(auto_step) = 1.0;
|
||||
}
|
||||
|
||||
static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
//struct ids_ctx_t * ctx = (struct ids_ctx_t *)ctx_ptr;
|
||||
struct ids_pin_ctx_t *pins = (struct ids_pin_ctx_t *)pin_ptr;
|
||||
|
||||
switch((int)(PIN(state) * 10.0 + 0.5)){
|
||||
case 0:
|
||||
break;
|
||||
|
||||
case 10:
|
||||
PIN(state) = 1.1;
|
||||
PIN(target) = PIN(max_pos);
|
||||
|
||||
if(PIN(auto_step) >= 1){
|
||||
PIN(state) = 1.2;
|
||||
}
|
||||
else{
|
||||
printf("Tune PPI bandwidth and damping\n");
|
||||
printf("the motor will move\n");
|
||||
printf("ids0.state = 1.2 to start\n");
|
||||
}
|
||||
break;
|
||||
|
||||
case 13:
|
||||
printf("conf0.pos_bw = %f # append to config\n", PIN(pos_bw));
|
||||
printf("conf0.vel_bw = %f # append to config\n", PIN(vel_bw));
|
||||
printf("conf0.vel_d = %f # append to config\n", PIN(vel_d));
|
||||
printf("done\n");
|
||||
PIN(state) = 1.4;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
//struct ids_ctx_t * ctx = (struct ids_ctx_t *)ctx_ptr;
|
||||
struct ids_pin_ctx_t *pins = (struct ids_pin_ctx_t *)pin_ptr;
|
||||
|
||||
if(PIN(en) <= 0.0){
|
||||
PIN(state) = 0.0;
|
||||
}
|
||||
|
||||
switch((int)(PIN(state) * 10.0 + 0.5)){
|
||||
case 0:
|
||||
PIN(acc_cmd) = 0.0;
|
||||
PIN(vel_cmd) = 0.0;
|
||||
PIN(acc) = 0.0;
|
||||
PIN(vel) = 0.0;
|
||||
|
||||
PIN(param) = 0.0;
|
||||
PIN(step) = 0.1;
|
||||
PIN(rep) = 1.0;
|
||||
|
||||
PIN(pos_bw) = 10.0;
|
||||
PIN(vel_bw) = 100.0;
|
||||
PIN(vel_d) = 10.0;
|
||||
PINA(params, 0) = PIN(vel_bw);
|
||||
PINA(params, 1) = 1.0 / PIN(vel_d);
|
||||
PINA(params, 2) = PIN(pos_bw);
|
||||
|
||||
PINA(max_params, 0) = PIN(cur_bw) / 2.0;
|
||||
PINA(max_params, 1) = 1.0;
|
||||
|
||||
PIN(min_cost) = (PIN(max_pos) - PIN(min_pos)) * (PIN(max_pos) - PIN(min_pos)) / PIN(max_vel) * 100.0;
|
||||
PIN(cost) = PIN(min_cost);
|
||||
|
||||
if(PIN(en) > 0.0){
|
||||
PIN(state) = 1.0;
|
||||
}
|
||||
break;
|
||||
|
||||
case 12:
|
||||
PIN(pos) += PIN(vel) * period + PIN(acc) * period * period / 2.0;
|
||||
PIN(vel) += PIN(acc) * period;
|
||||
float to_go = PIN(target) - PIN(pos);
|
||||
float time_to_go = sqrtf(2.0 * ABS(to_go) / PIN(max_acc));
|
||||
float acc = PIN(max_acc) * SIGN(to_go);
|
||||
float vel = acc * time_to_go;
|
||||
vel = LIMIT(vel, PIN(max_vel));
|
||||
acc = (vel - PIN(vel)) / period;
|
||||
|
||||
if(time_to_go < period / 2.0){
|
||||
time_to_go = 0.0;
|
||||
to_go = 0.0;
|
||||
vel = 0.0;
|
||||
acc = 0.0;
|
||||
PIN(pos) = PIN(target);
|
||||
PIN(vel) = 0.0;
|
||||
PIN(acc) = 0.0;
|
||||
}
|
||||
|
||||
PIN(acc) = LIMIT(acc, PIN(max_acc));
|
||||
|
||||
PIN(pos_cmd) = mod(PIN(pos));
|
||||
PIN(vel_cmd) = PIN(vel) * PIN(ff);
|
||||
PIN(acc_cmd) = PIN(acc) * PIN(ff);
|
||||
|
||||
PIN(cost) += ABS(PIN(pos_error)) * PIN(kp) * period;
|
||||
PIN(cost) += PIN(pos_error) * PIN(pos_error) * PIN(ks) * period;
|
||||
PIN(cost) += PIN(vel_error) * PIN(vel_error) * PIN(kv) * period;
|
||||
|
||||
|
||||
PIN(timer) += period;
|
||||
if(PIN(timer) < (ABS(PIN(max_pos) - PIN(min_pos)) / PIN(max_vel) + 2.0 * PIN(max_vel) / PIN(max_acc))){
|
||||
PIN(target) = PIN(max_pos);
|
||||
}
|
||||
else{
|
||||
PIN(target) = PIN(min_pos);
|
||||
}
|
||||
if(PIN(timer) > 2.0 * (ABS(PIN(max_pos) - PIN(min_pos)) / PIN(max_vel) + 2.0 * PIN(max_vel) / PIN(max_acc))){
|
||||
PIN(timer) = 0.0;
|
||||
}
|
||||
|
||||
if(PIN(timer) == 0.0){
|
||||
PIN(min_cost) = MIN(PIN(cost), PIN(min_cost));
|
||||
|
||||
PINA(max_params, 2) = PINA(params, 0) / 2.0;
|
||||
|
||||
if(PINA(params, (int)PIN(param)) > PINA(max_params, (int)PIN(param))){
|
||||
PINA(params, (int)PIN(param)) = PINA(max_params, (int)PIN(param));
|
||||
PIN(min_cost) = PIN(cost) * 10.0;
|
||||
PIN(param)++;
|
||||
}
|
||||
else if(PIN(cost) > PIN(min_cost) * PIN(kt)){
|
||||
PINA(params, (unsigned int)PIN(param)) *= PIN(kd);
|
||||
PIN(min_cost) = PIN(cost) * 10.0;
|
||||
PIN(param)++;
|
||||
}
|
||||
else{
|
||||
PINA(params, (unsigned int)PIN(param)) *= 1.0 + PIN(step);
|
||||
}
|
||||
|
||||
PIN(cost) = 0.0;
|
||||
}
|
||||
|
||||
PIN(pos_bw) = PINA(params, 2);
|
||||
PIN(vel_bw) = PINA(params, 0);
|
||||
PIN(vel_d) = 1.0 / PINA(params, 1);
|
||||
|
||||
if(PIN(param) > 2.0){
|
||||
PIN(acc_cmd) = 0.0;
|
||||
PIN(vel_cmd) = 0.0;
|
||||
PIN(param) = 0.0;
|
||||
|
||||
PIN(state) = 1.3;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
hal_comp_t ids_comp_struct = {
|
||||
.name = "ids",
|
||||
.nrt = nrt,
|
||||
.rt = rt_func,
|
||||
.frt = 0,
|
||||
.nrt_init = nrt_init,
|
||||
.rt_start = 0,
|
||||
.frt_start = 0,
|
||||
.rt_stop = 0,
|
||||
.frt_stop = 0,
|
||||
.ctx_size = 0,//sizeof(struct ids_ctx_t),
|
||||
.pin_count = sizeof(struct ids_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
|
||||
};
|
@ -221,7 +221,7 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
ctx->config.pins.r = PIN(r);
|
||||
ctx->config.pins.l = PIN(l);
|
||||
ctx->config.pins.psi = PIN(psi);
|
||||
ctx->config.pins.cur_bw = PIN(cur_bw);
|
||||
ctx->config.pins.cur_bw = PIN(cur_bw);
|
||||
ctx->config.pins.cur_ff = PIN(cur_ff);
|
||||
ctx->config.pins.cur_ind = PIN(cur_ind);
|
||||
ctx->config.pins.max_y = PIN(max_y);
|
||||
|
Loading…
Reference in New Issue
Block a user