📄 imvc07_a.lst
字号:
457 ; ei=sat(eim1+dyri-dyi)
458 ; dyri=ei-eim1+dyi
459 ; Pi=satsfl(Kps*dyri-Kps*dyi)
460 ; uim1=ui=sat(Pi+Iim1+uim1)
461 ; Iim1=satsfl(Kis*ei)
462 ; yrim1=yrim1+dyri
463 ; yim1=yi
464 ; eim1=ei
465 ; OVM=0
466 ; --------------------------
467 ; Calling Convention: extern void _pi_reg_omg();
468 ;==============================================================================
469 ; +--------------------------------------------------------------------------+
470 ; | Input global variables:
471 ; +--------------------------------------------------------------------------+
472 ; | _omg_ref | reference value of the speed at step i (yri)
473 ; | _omg | feedback value of the speed at step i (yi)
474 ; | _Kps_omg | scaled Kp coeff: Kps = Kp / (2^scalKp) so that
475 ; | | -1 < Kps < +1
TMS320C1x/C2x/C2xx/C5x COFF Assembler Version 7.00 Mon May 28 09:55:02 2007
Copyright (c) 1987-1999 Texas Instruments Incorporated
D:\Imvc\IMVC07_a.asm PAGE 12
476 ; | _sf_P_omg | sf_P_omg = scalKp
477 ; | _Kis_omg | scaled Ki coeff: Kis = Ki / (2^scalKi) so that
478 ; | | -1 < Kis < +1
479 ; | _sf_I_omg | sf_I_omg = scalKi
480 ; | _omg_ref_1 | reference value of the speed at step i-1 (yrim1)
481 ; | _omg_1 | feedback value of the speed at step i-1 (yim1)
482 ; | _e_omg_1 | error at step i-1
483 ; | _I_omg_1_high | integral term at step i-1, (16 MSB)
484 ; | _I_omg_1_low | integral term at step i-1, (16 LSB)
485 ; +--------------------------------------------------------------------------+
486 ; | Input local variables:
487 ; +--------------------------------------------------------------------------+
488 ; | dif_omg_ref | difference between actual and old reference
489 ; | dif_omg | difference between actual and old feedback
490 ; | e_omg | error at step i |
491 ; +--------------------------------------------------------------------------+
492 ; | Output global variables:
493 ; +--------------------------------------------------------------------------+
494 ; | _i_q_ref | output of the regulator (16 MSB)
495 ; | _iq_ref_low | output of the regulator (16 LSB)
496 ; +--------------------------------------------------------------------------+
497 ;------------------------------------------------------------------------------
498 .global _pi_reg_omg
499 ;==============================================================================
500 0126 _pi_reg_omg:
501 0126 be47 SETC SXM
502 0127 bc00- LDP #_omg
503 0128 be43 SETC OVM ; set overflow protection mode
504
505 0129 6a52- LACC _omg,16
506 012a 6559- SUB _omg_1,16
507 012b 9804- SACH dif_omg ; dyi=sat(yi-yim1)
508
509 012c 6a53- LACC _omg_ref,16
510 012d 6558- SUB _omg_ref_1,16
511 012e 9803- SACH dif_omg_ref ; dyri=sat(yri-yrim1)
512
513 012f 6504- SUB dif_omg,16 ; ACC=dyri-dyi
514 0130 615a- ADD _e_omg_1,16
515 0131 9805- SACH e_omg ; ei=sat(eim1+dyri-dyi)
516
517 0132 655a- SUB _e_omg_1,16
518 0133 6104- ADD dif_omg,16
519 0134 9803- SACH dif_omg_ref ; dyri=ei-eim1+dyi
520
521 0135 7354- LT _Kps_omg
522 0136 5403- MPY dif_omg_ref ; Kps*dyri
523 0137 be03 PAC
524 0138 5404- MPY dif_omg ; Kps*dyi
525 0139 be05 SPAC ; Kps*dyri-Kps*dyi
526 013a SATSFL _sf_P_omg ; ACC=Pi=satsfl(Kps*dyri-Kps*dyi) in Q31 format
1 013a 83a0 sar AR3, *+
1 013b 80ab sar AR0, *+,AR3
1 013c 0355- LAR AR3, _sf_P_omg
TMS320C1x/C2x/C2xx/C5x COFF Assembler Version 7.00 Mon May 28 09:55:02 2007
Copyright (c) 1987-1999 Texas Instruments Incorporated
D:\Imvc\IMVC07_a.asm PAGE 13
1 013d bf08 LAR AR0, #satvals12
013e 0026-
1 013f 8be0 MAR *0+
1 0140 6180 ADD *,16
1 0141 6580 SUB *,16
1 0142 6580 SUB *,16
1 0143 6189 ADD *,16,AR1
1 0144 0b55- RPT _sf_P_omg
1 0145 be09 SFL
1 0146 8b90 mar *-
1 0147 0090 lar AR0, *-
1 0148 0380 lar AR3, *
527 0149 be46 CLRC SXM
528 014a 205c- ADD _I_omg_1_low
529 014b 205d- ADD _iq_ref_low
530 014c be47 SETC SXM
531 014d 615b- ADD _I_omg_1_high,16
532 014e 6147- ADD _i_q_ref,16
533
534 014f sat_reg_out C_SAT_I_Q_REF ; macro that saturates regulator's output
1 014f bf9f ADD #C_SAT_I_Q_REF,15
0150 3334
1 0151 bfaf SUB #C_SAT_I_Q_REF,15
0152 3334
1 0153 bfaf SUB #C_SAT_I_Q_REF,15
0154 3334
1 0155 bf9f ADD #C_SAT_I_Q_REF,15
0156 3334
535
536 0157 9847- SACH _i_q_ref ; uim1=ui=sat(Pi+Iim1+uim1)
537 0158 905d- SACL _iq_ref_low
538
539 0159 7356- LT _Kis_omg
540 015a 5405- MPY e_omg
541 015b be03 PAC ; Kis*ei
542 015c SATSFL _sf_I_omg ; ACC=Iim1=satsfl(Kis*ei)
1 015c 83a0 sar AR3, *+
1 015d 80ab sar AR0, *+,AR3
1 015e 0357- LAR AR3, _sf_I_omg
1 015f bf08 LAR AR0, #satvals12
0160 0026-
1 0161 8be0 MAR *0+
1 0162 6180 ADD *,16
1 0163 6580 SUB *,16
1 0164 6580 SUB *,16
1 0165 6189 ADD *,16,AR1
1 0166 0b57- RPT _sf_I_omg
1 0167 be09 SFL
1 0168 8b90 mar *-
1 0169 0090 lar AR0, *-
1 016a 0380 lar AR3, *
543 016b 985b- SACH _I_omg_1_high ; Iim1=satsfl(Kis*ei)
544 016c 905c- SACL _I_omg_1_low
545
TMS320C1x/C2x/C2xx/C5x COFF Assembler Version 7.00 Mon May 28 09:55:02 2007
Copyright (c) 1987-1999 Texas Instruments Incorporated
D:\Imvc\IMVC07_a.asm PAGE 14
546 016d 6a58- LACC _omg_ref_1,16
547 016e 6103- ADD dif_omg_ref,16
548 016f 9858- SACH _omg_ref_1 ; yrim1=yrim1+dyri
549
550 0170 1052- LACC _omg
551 0171 9059- SACL _omg_1 ; yim1=yi
552
553 0172 1005- LACC e_omg
554 0173 905a- SACL _e_omg_1 ; eim1=ei
555
556 0174 be42 CLRC OVM ; disable overflow protection mode
557 0175 ef00 RET
558
559 ;=============================================================================
560 ; Routine Name: _start_encoder
561 ; ----------------------------
562 ; Description: Configure CAPCON to enable QEP circuit
563 ; ----------------------------
564 ; Calling Convention: extern void start_encoder();
565 ;=============================================================================
566 ; +-----------------+-------------------+-----------+
567 ; | Variables | on Entry | on Exit |
568 ; +-----------------+-------------------+-----------+
569 ; | ACC | xx |CAPCON Reg.|
570 ; | DP | xx | DP_EV |
571 ; +-----------------+-------------------+-----------+
572 ;-----------------------------------------------------------------------------
573 e000 CAPCON_QEP_EN .set 0E000h ; enable QEP in CAPCON register
574 .global _start_encoder
575 ;=============================================================================
576 0176 _start_encoder:
577 0176 bce8 LDP #DP_EV
578 0177 SETBIT T2CON,SETB6 ;start GPT2 to count QEP pulses
1 0177 6908 LACL T2CON
1 0178 bfc0 OR #SETB6
0179 0040
1 017a 9008 SACL T2CON
579 017b 1020 LACC CAPCONA
580 017c bfc0 OR #CAPCON_QEP_EN
017d e000
581 017e 9020 SACL CAPCONA ; enable QEP in CAPCON register
582 017f ef00 RET
583
584 ;=============================================================================
585 ; Routine Name: _read_encoder
586 ; ---------------------------
587 ; Description: Reads GPT2 counter (QEP capture pulses) and computes position.
588 ; The encoder position is loaded in the variable position
589 ; ---------------------------
590 ; Calling Convention: extern void _read_encoder();
591 ;=============================================================================
592 ; +-----------------+-------------------+-----------+
593 ; | Variables | on Entry | on Exit |
594 ; +-----------------+-------------------+-----------+
TMS320C1x/C2x/C2xx/C5x COFF Assembler Version 7.00 Mon May 28 09:55:02 2007
Copyright (c) 1987-1999 Texas Instruments Incorporated
D:\Imvc\IMVC07_a.asm PAGE 15
595 ; | ACC | xx | _position |
596 ; | DP | xx | DP_EV |
597 ; +-----------------+-------------------+-----------+
598 ;-----------------------------------------------------------------------------
599 .global _read_encoder
600 ;=============================================================================
601 0180 _read_encoder:
602 0180 bce8 LDP #DP_EV
603 0181 1005 LACC T2CNT ; read GPT2 current counter value
604 0182 bc00- LDP #_position
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -