diff --git a/conf/e240.txt b/conf/e240.txt new file mode 100644 index 00000000..5e3ee6f2 --- /dev/null +++ b/conf/e240.txt @@ -0,0 +1,30 @@ +conf0.r = 1.5 +conf0.l = 0.0025 +conf0.j = 0.00005 +conf0.psi = 0.04774 +conf0.polecount = 1 +conf0.mot_type = 3 +conf0.out_rev = 0.000000 + +conf0.max_vel = 837.75 +conf0.max_acc = 83775 +conf0.max_force = 2 +conf0.max_dc_cur = 10 +conf0.max_ac_cur = 6 + +conf0.fb_type = 1 +conf0.fb_res = 2048 + +conf0.cmd_type = 1 +conf0.cmd_res = 2000 + +conf0.p = 1 +conf0.pos_p = 150 +conf0.vel_p = 1 +conf0.acc_p = 1 +conf0.acc_pi = 100 +conf0.cur_p = 0.5 +conf0.cur_i = 0.001 +conf0.cur_ff = 1 +conf0.cur_ind = 0.9 +conf0.max_sat = 0.2 diff --git a/shared/common.h b/shared/common.h index afd2e5f6..71a0b069 100644 --- a/shared/common.h +++ b/shared/common.h @@ -32,6 +32,7 @@ typedef struct{ int16_t dc_cur; int16_t dc_volt; int16_t hv_temp; + uint16_t status; #ifdef TROLLER int16_t a; int16_t b; @@ -43,6 +44,7 @@ typedef struct{ typedef struct{ int16_t a; int16_t b; + uint16_t mode; } to_hv_t; typedef struct{ diff --git a/src/comps/hv.comp b/src/comps/hv.comp index f8c1b2ca..fd7dbf04 100644 --- a/src/comps/hv.comp +++ b/src/comps/hv.comp @@ -2,6 +2,7 @@ HAL_COMP(hv); HAL_PIN(a) = 0.0; HAL_PIN(b) = 0.0; +HAL_PIN(mode) = 0.0; HAL_PIN(enable) = 0.0; HAL_PIN(error) = 0.0; @@ -147,6 +148,7 @@ RT( if(e > 0.0){ packet_to_hv.data.a = TOFIXED(a); packet_to_hv.data.b = TOFIXED(b); + packet_to_hv.data.mode = PIN(mode); } else{ packet_to_hv.data.a = 0; diff --git a/src/comps/term.comp b/src/comps/term.comp index 2fdaec62..b58a74a9 100644 --- a/src/comps/term.comp +++ b/src/comps/term.comp @@ -102,6 +102,7 @@ int load(){ //update cmd/fb links update_cmd(); update_fb(); + update_mot(); return 0; } diff --git a/src/link.c b/src/link.c index 65088c47..4eaa3d58 100644 --- a/src/link.c +++ b/src/link.c @@ -203,6 +203,28 @@ void link_pid(){ hal_link_pins("conf0.cos_offset", "adc0.cos_offset"); } +int update_mot(){ + switch((mot_type_t)hal_get_pin("conf0.mot_type")){ + case ACSYNC: + //TODO: copy acsync stuff from link_pid here + hal_link_pins("curpid0.uq", "idq0.q"); + hal_set_pin("hv0.mode", 0.0); + break; + case ACASYNC: + break; + case AC2PHASE: + break; + case DC: + //TODO: implement proper dc model + hal_link_pins("curpid0.uq", "hv0.a"); + hal_set_pin("hv0.mode", 1.0); + break; + default: + return -1; + } + return 0; +} + int update_fb(){ hal_set_pin("adc0.rt_prio", -1.0); hal_set_pin("enc_fb0.rt_prio", -1.0); @@ -280,7 +302,7 @@ int update_cmd(){ //this breaks cmd rev... hal_link_pins("vel_int0.pos_out", "net0.cmd"); hal_link_pins("vel_int0.vel_out", "net0.cmd_d"); - hal_linkpin("vel_int0.wd", 0.002);//TODO: this depends on linuxcnc servo thread period + hal_set_pin("vel_int0.wd", 0.002);//TODO: this depends on linuxcnc servo thread period //TODO: handle error of vel_int hal_set_pin("sserial0.rt_prio", 2.0); hal_set_pin("sserial0.frt_prio", 2.0); diff --git a/src/link.h b/src/link.h index b25a43c6..0fd893a6 100644 --- a/src/link.h +++ b/src/link.h @@ -19,6 +19,14 @@ typedef enum{ SSI } protocol_t; +typedef enum{ + ACSYNC = 0, + ACASYNC, + AC2PHASE, + DC +} mot_type_t; + void link_pid(); int update_cmd(); int update_fb(); +int update_mot(); diff --git a/stm32f103/src/main.c b/stm32f103/src/main.c index ce7ec17d..59151538 100644 --- a/stm32f103/src/main.c +++ b/stm32f103/src/main.c @@ -409,44 +409,55 @@ void USART2_IRQHandler(){ float ua = TOFLOAT(packet_to_hv.data.a); float ub = TOFLOAT(packet_to_hv.data.b); - float u = ua; // inverse clarke - float v = - ua / 2.0 + ub / 2.0 * SQRT3; - float w = - ua / 2.0 - ub / 2.0 * SQRT3; + if(packet_to_hv.data.mode == 0){//a,b voltages + float u = ua; // inverse clarke + float v = - ua / 2.0 + ub / 2.0 * SQRT3; + float w = - ua / 2.0 - ub / 2.0 * SQRT3; - //TODO: clamping - u += volt / 2.0; - v += volt / 2.0; - w += volt / 2.0; + //TODO: clamping + u += volt / 2.0; + v += volt / 2.0; + w += volt / 2.0; - if(u < v){ - if(u < w){ - v -= u; - w -= u; - u = 0.0; - } - else{ - u -= w; - v -= w; - w = 0.0; - } - } - else{ - if(v < w){ - u -= v; - w -= v; - v = 0.0; - } - else{ - u -= w; - v -= w; - w = 0.0; - } - } - - PWM_U = CLAMP(u / volt * PWM_RES, 0, PWM_RES * 0.95); - PWM_V = CLAMP(v / volt * PWM_RES, 0, PWM_RES * 0.95); - PWM_W = CLAMP(w / volt * PWM_RES, 0, PWM_RES * 0.95); + if(u < v){ + if(u < w){ + v -= u; + w -= u; + u = 0.0; + } + else{ + u -= w; + v -= w; + w = 0.0; + } + } + else{ + if(v < w){ + u -= v; + w -= v; + v = 0.0; + } + else{ + u -= w; + v -= w; + w = 0.0; + } + } + PWM_U = CLAMP(u / volt * PWM_RES, 0, PWM_RES * 0.95); + PWM_V = CLAMP(v / volt * PWM_RES, 0, PWM_RES * 0.95); + PWM_W = CLAMP(w / volt * PWM_RES, 0, PWM_RES * 0.95); + }else if(packet_to_hv.data.mode == 1){//DC, a: -dclink ... +dclink + ua += volt; + PWM_U = CLAMP(ua / (volt*2.0) * PWM_RES, 0, PWM_RES * 0.95); + PWM_V = CLAMP((1.0 - (ua / (volt*2.0))) * PWM_RES, 0, PWM_RES * 0.95); + PWM_W = 0; + }else{ + PWM_U = 0; + PWM_V = 0; + PWM_W = 0; + } + timeout = 0; //reset timeout } }