标题: PID SERVO Encoder control de posicion motor [打印本页]
作者: veerakumars    时间: 2018-6-28 12:02
标题: PID SERVO Encoder control de posicion motor
PID SERVO CONTROL
仿真原理图如下（proteus仿真工程文件可到本帖附件中下载）
SET POINT -50 A 50  PULSOS
CORRESPONDE -180 A 180 GRADOS


单片机源程序如下:
  1. _pid:

  3. ;ENCODER.c,31 ::                 void pid(float pid_in)
  4. ;ENCODER.c,38 ::                 error=setpoint-pid_in;
  5.         MOVF       FARG_pid_pid_in+0, 0
  6.         MOVWF      R4+0
  7.         MOVF       FARG_pid_pid_in+1, 0
  8.         MOVWF      R4+1
  9.         MOVF       FARG_pid_pid_in+2, 0
  10.         MOVWF      R4+2
  11.         MOVF       FARG_pid_pid_in+3, 0
  12.         MOVWF      R4+3
  13.         MOVF       _setpoint+0, 0
  14.         MOVWF      R0+0
  15.         MOVF       _setpoint+1, 0
  16.         MOVWF      R0+1
  17.         MOVF       _setpoint+2, 0
  18.         MOVWF      R0+2
  19.         MOVF       _setpoint+3, 0
  20.         MOVWF      R0+3
  21.         CALL       _Sub_32x32_FP+0
  22.         MOVF       R0+0, 0
  23.         MOVWF      _error+0
  24.         MOVF       R0+1, 0
  25.         MOVWF      _error+1
  26.         MOVF       R0+2, 0
  27.         MOVWF      _error+2
  28.         MOVF       R0+3, 0
  29.         MOVWF      _error+3
  30. ;ENCODER.c,39 ::                 up=kp*error;
  31.         MOVF       _kp+0, 0
  32.         MOVWF      R4+0
  33.         MOVF       _kp+1, 0
  34.         MOVWF      R4+1
  35.         MOVF       _kp+2, 0
  36.         MOVWF      R4+2
  37.         MOVF       _kp+3, 0
  38.         MOVWF      R4+3
  39.         CALL       _Mul_32x32_FP+0
  40.         MOVF       R0+0, 0
  41.         MOVWF      _up+0
  42.         MOVF       R0+1, 0
  43.         MOVWF      _up+1
  44.         MOVF       R0+2, 0
  45.         MOVWF      _up+2
  46.         MOVF       R0+3, 0
  47.         MOVWF      _up+3
  48. ;ENCODER.c,40 ::                 if (ti!=0) ki=(kp/ti);
  49.         CLRF       R4+0
  50.         CLRF       R4+1
  51.         CLRF       R4+2
  52.         CLRF       R4+3
  53.         MOVF       _ti+0, 0
  54.         MOVWF      R0+0
  55.         MOVF       _ti+1, 0
  56.         MOVWF      R0+1
  57.         MOVF       _ti+2, 0
  58.         MOVWF      R0+2
  59.         MOVF       _ti+3, 0
  60.         MOVWF      R0+3
  61.         CALL       _Equals_Double+0
  62.         MOVLW      1
  63.         BTFSC      STATUS+0, 2
  64.         MOVLW      0
  65.         MOVWF      R0+0
  66.         MOVF       R0+0, 0
  67.         BTFSC      STATUS+0, 2
  68.         GOTO       L_pid0
  69.         MOVF       _ti+0, 0
  70.         MOVWF      R4+0
  71.         MOVF       _ti+1, 0
  72.         MOVWF      R4+1
  73.         MOVF       _ti+2, 0
  74.         MOVWF      R4+2
  75.         MOVF       _ti+3, 0
  76.         MOVWF      R4+3
  77.         MOVF       _kp+0, 0
  78.         MOVWF      R0+0
  79.         MOVF       _kp+1, 0
  80.         MOVWF      R0+1
  81.         MOVF       _kp+2, 0
  82.         MOVWF      R0+2
  83.         MOVF       _kp+3, 0
  84.         MOVWF      R0+3
  85.         CALL       _Div_32x32_FP+0
  86.         MOVF       R0+0, 0
  87.         MOVWF      _ki+0
  88.         MOVF       R0+1, 0
  89.         MOVWF      _ki+1
  90.         MOVF       R0+2, 0
  91.         MOVWF      _ki+2
  92.         MOVF       R0+3, 0
  93.         MOVWF      _ki+3
  94.         GOTO       L_pid1
  95. L_pid0:
  96. ;ENCODER.c,41 ::                 else ki=0;
  97.         CLRF       _ki+0
  98.         CLRF       _ki+1
  99.         CLRF       _ki+2
  100.         CLRF       _ki+3
  101. L_pid1:
  102. ;ENCODER.c,42 ::                 ui=ui_ + (ki*t_muestreo*error);
  103.         MOVF       _ki+0, 0
  104.         MOVWF      R0+0
  105.         MOVF       _ki+1, 0
  106.         MOVWF      R0+1
  107.         MOVF       _ki+2, 0
  108.         MOVWF      R0+2
  109.         MOVF       _ki+3, 0
  110.         MOVWF      R0+3
  111.         MOVF       _error+0, 0
  112.         MOVWF      R4+0
  113.         MOVF       _error+1, 0
  114.         MOVWF      R4+1
  115.         MOVF       _error+2, 0
  116.         MOVWF      R4+2
  117.         MOVF       _error+3, 0
  118.         MOVWF      R4+3
  119.         CALL       _Mul_32x32_FP+0
  120.         MOVF       _ui_+0, 0
  121.         MOVWF      R4+0
  122.         MOVF       _ui_+1, 0
  123.         MOVWF      R4+1
  124.         MOVF       _ui_+2, 0
  125.         MOVWF      R4+2
  126.         MOVF       _ui_+3, 0
  127.         MOVWF      R4+3
  128.         CALL       _Add_32x32_FP+0
  129.         MOVF       R0+0, 0
  130.         MOVWF      FLOC__pid+4
  131.         MOVF       R0+1, 0
  132.         MOVWF      FLOC__pid+5
  133.         MOVF       R0+2, 0
  134.         MOVWF      FLOC__pid+6
  135.         MOVF       R0+3, 0
  136.         MOVWF      FLOC__pid+7
  137.         MOVF       FLOC__pid+4, 0
  138.         MOVWF      _ui+0
  139.         MOVF       FLOC__pid+5, 0
  140.         MOVWF      _ui+1
  141.         MOVF       FLOC__pid+6, 0
  142.         MOVWF      _ui+2
  143.         MOVF       FLOC__pid+7, 0
  144.         MOVWF      _ui+3
  145. ;ENCODER.c,44 ::                 kd=kp*td;
  146.         MOVF       _kp+0, 0
  147.         MOVWF      R0+0
  148.         MOVF       _kp+1, 0
  149.         MOVWF      R0+1
  150.         MOVF       _kp+2, 0
  151.         MOVWF      R0+2
  152.         MOVF       _kp+3, 0
  153.         MOVWF      R0+3
  154.         MOVF       _td+0, 0
  155.         MOVWF      R4+0
  156.         MOVF       _td+1, 0
  157.         MOVWF      R4+1
  158.         MOVF       _td+2, 0
  159.         MOVWF      R4+2
  160.         MOVF       _td+3, 0
  161.         MOVWF      R4+3
  162.         CALL       _Mul_32x32_FP+0
  163.         MOVF       R0+0, 0
  164.         MOVWF      FLOC__pid+0
  165.         MOVF       R0+1, 0
  166.         MOVWF      FLOC__pid+1
  167.         MOVF       R0+2, 0
  168.         MOVWF      FLOC__pid+2
  169.         MOVF       R0+3, 0
  170.         MOVWF      FLOC__pid+3
  171.         MOVF       FLOC__pid+0, 0
  172.         MOVWF      _kd+0
  173.         MOVF       FLOC__pid+1, 0
  174.         MOVWF      _kd+1
  175.         MOVF       FLOC__pid+2, 0
  176.         MOVWF      _kd+2
  177.         MOVF       FLOC__pid+3, 0
  178.         MOVWF      _kd+3
  179. ;ENCODER.c,45 ::                 d_error=error-error_;
  180.         MOVF       _error_+0, 0
  181.         MOVWF      R4+0
  182.         MOVF       _error_+1, 0
  183.         MOVWF      R4+1
  184.         MOVF       _error_+2, 0
  185.         MOVWF      R4+2
  186.         MOVF       _error_+3, 0
  187.         MOVWF      R4+3
  188.         MOVF       _error+0, 0
  189.         MOVWF      R0+0
  190.         MOVF       _error+1, 0
  191.         MOVWF      R0+1
  192.         MOVF       _error+2, 0
  193.         MOVWF      R0+2
  194.         MOVF       _error+3, 0
  195.         MOVWF      R0+3
  196.         CALL       _Sub_32x32_FP+0
  197.         MOVF       R0+0, 0
  198.         MOVWF      _d_error+0
  199.         MOVF       R0+1, 0
  200.         MOVWF      _d_error+1
  201.         MOVF       R0+2, 0
  202.         MOVWF      _d_error+2
  203.         MOVF       R0+3, 0
  204.         MOVWF      _d_error+3
  205. ;ENCODER.c,46 ::                 ud=kd*(d_error)/t_muestreo;
  206.         MOVF       FLOC__pid+0, 0
  207.         MOVWF      R4+0
  208.         MOVF       FLOC__pid+1, 0
  209.         MOVWF      R4+1
  210.         MOVF       FLOC__pid+2, 0
  211.         MOVWF      R4+2
  212.         MOVF       FLOC__pid+3, 0
  213.         MOVWF      R4+3
  214.         CALL       _Mul_32x32_FP+0
  215.         MOVF       R0+0, 0
  216.         MOVWF      FLOC__pid+0
  217.         MOVF       R0+1, 0
  218.         MOVWF      FLOC__pid+1
  219.         MOVF       R0+2, 0
  220.         MOVWF      FLOC__pid+2
  221.         MOVF       R0+3, 0
  222.         MOVWF      FLOC__pid+3
  223.         MOVF       FLOC__pid+0, 0
  224.         MOVWF      _ud+0
  225.         MOVF       FLOC__pid+1, 0
  226.         MOVWF      _ud+1
  227.         MOVF       FLOC__pid+2, 0
  228.         MOVWF      _ud+2
  229.         MOVF       FLOC__pid+3, 0
  230.         MOVWF      _ud+3
  231. ;ENCODER.c,48 ::                 ut=up+ui+ud;
  232.         MOVF       _up+0, 0
  233.         MOVWF      R0+0
  234.         MOVF       _up+1, 0
  235.         MOVWF      R0+1
  236.         MOVF       _up+2, 0
  237.         MOVWF      R0+2
  238.         MOVF       _up+3, 0
  239.         MOVWF      R0+3
  240.         MOVF       FLOC__pid+4, 0
  241.         MOVWF      R4+0
  242.         MOVF       FLOC__pid+5, 0
  243.         MOVWF      R4+1
  244.         MOVF       FLOC__pid+6, 0
  245.         MOVWF      R4+2
  246.         MOVF       FLOC__pid+7, 0
  247.         MOVWF      R4+3
  248.         CALL       _Add_32x32_FP+0
  249.         MOVF       FLOC__pid+0, 0
  250.         MOVWF      R4+0
  251.         MOVF       FLOC__pid+1, 0
  252.         MOVWF      R4+1
  253.         MOVF       FLOC__pid+2, 0
  254.         MOVWF      R4+2
  255.         MOVF       FLOC__pid+3, 0
  256.         MOVWF      R4+3
  257.         CALL       _Add_32x32_FP+0
  258.         MOVF       R0+0, 0
  259.         MOVWF      _ut+0
  260.         MOVF       R0+1, 0
  261.         MOVWF      _ut+1
  262.         MOVF       R0+2, 0
  263.         MOVWF      _ut+2
  264.         MOVF       R0+3, 0
  265.         MOVWF      _ut+3
  266. ;ENCODER.c,50 ::                 if (ut>max_x) ut=max_x;
  267.         MOVF       R0+0, 0
  268.         MOVWF      R4+0
  269.         MOVF       R0+1, 0
  270.         MOVWF      R4+1
  271.         MOVF       R0+2, 0
  272.         MOVWF      R4+2
  273.         MOVF       R0+3, 0
  274.         MOVWF      R4+3
  275.         MOVLW      0
  276.         MOVWF      R0+0
  277.         MOVLW      0
  278.         MOVWF      R0+1
  279.         MOVLW      127
  280.         MOVWF      R0+2
  281.         MOVLW      134
  282.         MOVWF      R0+3
  283.         CALL       _Compare_Double+0
  284.         MOVLW      1
  285.         BTFSC      STATUS+0, 0
  286.         MOVLW      0
  287.         MOVWF      R0+0
  288.         MOVF       R0+0, 0
  289.         BTFSC      STATUS+0, 2
  290.         GOTO       L_pid2
  291.         MOVLW      0
  292.         MOVWF      _ut+0
  293.         MOVLW      0
  294.         MOVWF      _ut+1
  295.         MOVLW      127
  296.         MOVWF      _ut+2
  297.         MOVLW      134
  298.         MOVWF      _ut+3
  299. L_pid2:
  300. ;ENCODER.c,51 ::                 if (ut<min_x) ut=min_x;
  301.         MOVLW      0
  302.         MOVWF      R4+0
  303.         MOVLW      0
  304.         MOVWF      R4+1
  305.         MOVLW      255
  306.         MOVWF      R4+2
  307.         MOVLW      134
  308.         MOVWF      R4+3
  309.         MOVF       _ut+0, 0
  310.         MOVWF      R0+0
  311.         MOVF       _ut+1, 0
  312.         MOVWF      R0+1
  313.         MOVF       _ut+2, 0
  314.         MOVWF      R0+2
  315.         MOVF       _ut+3, 0
  316.         MOVWF      R0+3
  317.         CALL       _Compare_Double+0
  318.         MOVLW      1
  319.         BTFSC      STATUS+0, 0
  320.         MOVLW      0
  321.         MOVWF      R0+0
  322.         MOVF       R0+0, 0
  323.         BTFSC      STATUS+0, 2
  324.         GOTO       L_pid3
  325.         MOVLW      0
  326.         MOVWF      _ut+0
  327.         MOVLW      0
  328.         MOVWF      _ut+1
  329.         MOVLW      255
  330.         MOVWF      _ut+2
  331.         MOVLW      134
  332.         MOVWF      _ut+3
  333. L_pid3:
  334. ;ENCODER.c,53 ::                 ui_=ui;
  335.         MOVF       _ui+0, 0
  336.         MOVWF      _ui_+0
  337.         MOVF       _ui+1, 0
  338.         MOVWF      _ui_+1
  339.         MOVF       _ui+2, 0
  340.         MOVWF      _ui_+2
  341.         MOVF       _ui+3, 0
  342.         MOVWF      _ui_+3
  343. ;ENCODER.c,54 ::                 error_=error;
  344.         MOVF       _error+0, 0
  345.         MOVWF      _error_+0
  346.         MOVF       _error+1, 0
  347.         MOVWF      _error_+1
  348.         MOVF       _error+2, 0
  349.         MOVWF      _error_+2
  350.         MOVF       _error+3, 0
  351.         MOVWF      _error_+3
  352. ;ENCODER.c,56 ::                 }
  353. L_end_pid:
  354.         RETURN
  355. ; end of _pid

  357. _Interrpciones:
  358.         MOVWF      R15+0
  359.         SWAPF      STATUS+0, 0
  360.         CLRF       STATUS+0
  361.         MOVWF      ___saveSTATUS+0
  362.         MOVF       PCLATH+0, 0
  363.         MOVWF      ___savePCLATH+0
  364.         CLRF       PCLATH+0

  366. ;ENCODER.c,64 ::                 void Interrpciones() iv 0x0004 ics ICS_AUTO
  367. ;ENCODER.c,66 ::                 if (INTF_bit==1)
  368.         BTFSS      INTF_bit+0, BitPos(INTF_bit+0)
  369.         GOTO       L_Interrpciones4
  370. ;ENCODER.c,68 ::                 if (PORTB.RB1==PORTB.RB0) pulsos++; else pulsos--;
  371.         BTFSC      PORTB+0, 1
  372.         GOTO       L__Interrpciones17
  373.         BTFSS      PORTB+0, 0
  374.         GOTO       L__Interrpciones18
  375.         GOTO       L_Interrpciones5
  376. L__Interrpciones17:
  377.         BTFSS      PORTB+0, 0
  378.         GOTO       L_Interrpciones5
  379. L__Interrpciones18:
  380.         INCF       _pulsos+0, 1
  381.         BTFSC      STATUS+0, 2
  382.         INCF       _pulsos+1, 1
  383.         GOTO       L_Interrpciones6
  384. L_Interrpciones5:
  385.         MOVLW      1
  386.         SUBWF      _pulsos+0, 1
  387.         BTFSS      STATUS+0, 0
  388.         DECF       _pulsos+1, 1
  389. L_Interrpciones6:
  390. ;ENCODER.c,69 ::                 if (pulsos>50) PULSOS=50;
  391.         MOVLW      128
  392.         MOVWF      R0+0
  393.         MOVLW      128
  394.         XORWF      _pulsos+1, 0
  395.         SUBWF      R0+0, 0
  396.         BTFSS      STATUS+0, 2
  397.         GOTO       L__Interrpciones19
  398.         MOVF       _pulsos+0, 0
  399.         SUBLW      50
  400. L__Interrpciones19:
  401.         BTFSC      STATUS+0, 0
  402.         GOTO       L_Interrpciones7
  403.         MOVLW      50
  404.         MOVWF      _pulsos+0
  405.         MOVLW      0
  406.         MOVWF      _pulsos+1
  407.         GOTO       L_Interrpciones8
  408. L_Interrpciones7:
  409. ;ENCODER.c,70 ::                 else if  (pulsos<-50) PULSOS=-50;
  410.         MOVLW      128
  411.         XORWF      _pulsos+1, 0
  412.         MOVWF      R0+0
  413.         MOVLW      127
  414.         SUBWF      R0+0, 0
  415.         BTFSS      STATUS+0, 2
  416.         GOTO       L__Interrpciones20
  417.         MOVLW      206
  418.         SUBWF      _pulsos+0, 0
  419. L__Interrpciones20:
  420.         BTFSC      STATUS+0, 0
  421.         GOTO       L_Interrpciones9
  422.         MOVLW      206
  423.         MOVWF      _pulsos+0
  424.         MOVLW      255
  425.         MOVWF      _pulsos+1
  426. L_Interrpciones9:
  427. L_Interrpciones8:
  428. ;ENCODER.c,71 ::                 INTEDG_bit=~INTEDG_bit; //cambia el flanco a detectar modo 2x
  429.         MOVLW
  430.         XORWF      INTEDG_bit+0, 1
  431. ;ENCODER.c,72 ::                 INTF_bit=0; //limpia bandera de interrupcion
  432.         BCF        INTF_bit+0, BitPos(INTF_bit+0)
  433. ;ENCODER.c,73 ::                 }
  434. L_Interrpciones4:
  435. ;ENCODER.c,75 ::                 }
  436. L_end_Interrpciones:
  437. L__Interrpciones16:
  438.         MOVF       ___savePCLATH+0, 0
  439.         MOVWF      PCLATH+0
  440.         SWAPF      ___saveSTATUS+0, 0
  441.         MOVWF      STATUS+0
  442.         SWAPF      R15+0, 1
  443.         SWAPF      R15+0, 0
  444.         RETFIE
  445. ; end of _Interrpciones

  447. _main:

  449. ;ENCODER.c,77 ::                 void main()
  450. ;ENCODER.c,79 ::                 TRISB=255;  //TODO ENTRADAS
  451.         MOVLW      255
  452.         MOVWF      TRISB+0
  453. ;ENCODER.c,80 ::                 TRISC=0;    //TODOS SALIDAS
  454.         CLRF       TRISC+0
  455. ;ENCODER.c,81 ::                 trisa=255; //todo entradas
  456.         MOVLW      255
  457.         MOVWF      TRISA+0
  458. ;ENCODER.c,83 ::                 trisb.b3=0;   //salida control de giro
  459.         BCF        TRISB+0, 3
  460. ;ENCODER.c,84 ::                 trisb.b4=0;  //Salida Contro de giro
  461.         BCF        TRISB+0, 4
  462. ;ENCODER.c,87 ::                 adcon0.b0=0;
  463.         BCF        ADCON0+0, 0
  464. ;ENCODER.c,88 ::                 adcon1=0;
  465.         CLRF       ADCON1+0
  466. ;ENCODER.c,89 ::                 Lcd_Init();                        // Initialize LCD
  467.         CALL       _Lcd_Init+0
  468. ;ENCODER.c,91 ::                 Lcd_Cmd(_LCD_CLEAR);               // Clear display
  469.         MOVLW      1
  470.         MOVWF      FARG_Lcd_Cmd_out_char+0
  471.         CALL       _Lcd_Cmd+0
  472. ;ENCODER.c,92 ::                 Lcd_Cmd(_LCD_CURSOR_OFF);          // Cursor off
  473.         MOVLW      12
  474.         MOVWF      FARG_Lcd_Cmd_out_char+0
  475.         CALL       _Lcd_Cmd+0
  476. ;ENCODER.c,93 ::                 Lcd_Out(1,1,"    CONTROL ARZ");
  477.         MOVLW      1
  478.         MOVWF      FARG_Lcd_Out_row+0
  479.         MOVLW      1
  480.         MOVWF      FARG_Lcd_Out_column+0
  481.         MOVLW      ?lstr1_ENCODER+0
  482.         MOVWF      FARG_Lcd_Out_text+0
  483.         CALL       _Lcd_Out+0
  484. ;ENCODER.c,94 ::                 Lcd_Out(2,1,"SET_POINT=");
  485.         MOVLW      2
  486.         MOVWF      FARG_Lcd_Out_row+0
  487.         MOVLW      1
  488.         MOVWF      FARG_Lcd_Out_column+0
  489.         MOVLW      ?lstr2_ENCODER+0
  490.         MOVWF      FARG_Lcd_Out_text+0
  491.         CALL       _Lcd_Out+0
  492. ;ENCODER.c,95 ::                 Lcd_Out(4,1,"PULSOS=");
  493.         MOVLW      4
  494.         MOVWF      FARG_Lcd_Out_row+0
  495.         MOVLW      1
  496.         MOVWF      FARG_Lcd_Out_column+0
  497.         MOVLW      ?lstr3_ENCODER+0
  498.         MOVWF      FARG_Lcd_Out_text+0
  499.         CALL       _Lcd_Out+0
  500. ;ENCODER.c,97 ::                 ADC_Init();
  501.         CALL       _ADC_Init+0
  502. ;ENCODER.c,98 ::                 PWM1_Init(400);
  503.         BSF        T2CON+0, 0
  504.         BSF        T2CON+0, 1
  505.         MOVLW      156
  506.         MOVWF      PR2+0
  507.         CALL       _PWM1_Init+0
  508. ;ENCODER.c,99 ::                 PWM1_Set_Duty(0);
  509.         CLRF       FARG_PWM1_Set_Duty_new_duty+0
  510.         CALL       _PWM1_Set_Duty+0
  511. ;ENCODER.c,100 ::                 PWM1_Start();
  512.         CALL       _PWM1_Start+0
  513. ;ENCODER.c,105 ::                 INTCON=0B01000000;
  514.         MOVLW      64
  515.         MOVWF      INTCON+0
  516. ;ENCODER.c,107 ::                 INTF_bit=0; //limpia la bansera e interrupcion
  517.         BCF        INTF_bit+0, BitPos(INTF_bit+0)
  518. ;ENCODER.c,108 ::                 INTEDG_bit=0; //por flanco de bajada
  519.         BCF        INTEDG_bit+0, BitPos(INTEDG_bit+0)
  520. ;ENCODER.c,109 ::                 INTE_bit=1; //habilita interrpcion por flanco
  521.         BSF        INTE_bit+0, BitPos(INTE_bit+0)
  522. ;ENCODER.c,110 ::                 GIE_bit=1; //Autoriza interrupciones
  523.         BSF        GIE_bit+0, BitPos(GIE_bit+0)
  524. ;ENCODER.c,116 ::                 while (1)
  525. L_main10:
  526. ;ENCODER.c,118 ::                 codigo=ADC_Get_Sample(0);
  527.         CLRF       FARG_ADC_Get_Sample_channel+0
  528.         CALL       _ADC_Get_Sample+0
  529.         MOVF       R0+0, 0
  530.         MOVWF      _codigo+0
  531.         MOVF       R0+1, 0
  532.         MOVWF      _codigo+1
  533. ;ENCODER.c,121 ::                 setpoint=  0.0978*codigo-50.0;
  534.         CALL       _word2double+0
  535.         MOVLW      94
  536.         MOVWF      R4+0
  537.         MOVLW      75
  538.         MOVWF      R4+1
  539.         MOVLW      72
  540.         MOVWF      R4+2
  541.         MOVLW      123
  542.         MOVWF      R4+3
  543.         CALL       _Mul_32x32_FP+0
  544.         MOVLW      0
  545.         MOVWF      R4+0
  546.         MOVLW      0
  547.         MOVWF      R4+1
  548.         MOVLW      72
  549.         MOVWF      R4+2
  550.         MOVLW      132
  551.         MOVWF      R4+3
  552.         CALL       _Sub_32x32_FP+0
  553.         MOVF       R0+0, 0
  554.         MOVWF      _setpoint+0
  555.         MOVF       R0+1, 0
  556.         MOVWF      _setpoint+1
  557.         MOVF       R0+2, 0
  558.         MOVWF      _setpoint+2
  559.         MOVF       R0+3, 0
  560.         MOVWF      _setpoint+3
  561. ;ENCODER.c,123 ::                 pid(pulsos);
  562.         MOVF       _pulsos+0, 0
  563.         MOVWF      R0+0
  564.         MOVF       _pulsos+1, 0
  565.         MOVWF      R0+1
  566.         CALL       _int2double+0
  567.         MOVF       R0+0, 0
  568.         MOVWF      FARG_pid_pid_in+0
  569.         MOVF       R0+1, 0
  570.         MOVWF      FARG_pid_pid_in+1
  571.         MOVF       R0+2, 0
  572.         MOVWF      FARG_pid_pid_in+2
  573.         MOVF       R0+3, 0
  574.         MOVWF      FARG_pid_pid_in+3
  575.         CALL       _pid+0
  576. ;ENCODER.c,125 ::                 if (ut<0)    //gira a la derecha     analiza la salida para determinar giro
  577.         CLRF       R4+0
  578.         CLRF       R4+1
  579.         CLRF       R4+2
  580.         CLRF       R4+3
  581.         MOVF       _ut+0, 0
  582.         MOVWF      R0+0
  583.         MOVF       _ut+1, 0
  584.         MOVWF      R0+1
  585.         MOVF       _ut+2, 0
  586.         MOVWF      R0+2
  587.         MOVF       _ut+3, 0
  588.         MOVWF      R0+3
  589.         CALL       _Compare_Double+0
  590.         MOVLW      1
  591.         BTFSC      STATUS+0, 0
  592.         MOVLW      0
  593.         MOVWF      R0+0
  594.         MOVF       R0+0, 0
  595.         BTFSC      STATUS+0, 2
  596.         GOTO       L_main12
  597. ;ENCODER.c,127 ::                 ut=-ut;   // el valor del pwm en cada sentido debe ser solo positivo
  598.         MOVLW      0
  599.         XORWF      _ut+0, 1
  600.         MOVLW      0
  601.         XORWF      _ut+1, 1
  602.         MOVLW      128
  603.         XORWF      _ut+2, 1
  604.         MOVLW      0
  605.         XORWF      _ut+3, 1
  606. ;ENCODER.c,128 ::                 m11=1;
  607.         BSF        PORTB+0, 3
  608. ;ENCODER.c,129 ::                 m12=0;
  609.         BCF        PORTB+0, 4
  610. ;ENCODER.c,130 ::                 }
  611.         GOTO       L_main13
  612. L_main12:
  613. ;ENCODER.c,133 ::                 m11=0;
  614.         BCF        PORTB+0, 3
  615. ;ENCODER.c,134 ::                 m12=1;
  616.         BSF        PORTB+0, 4
  617. ;ENCODER.c,136 ::                 }
  618. L_main13:
  619. ;ENCODER.c,140 ::                 PWM1_Set_Duty(ut); //salida pid   po PWM
  620.         MOVF       _ut+0, 0
  621.         MOVWF      R0+0
  622.         MOVF       _ut+1, 0
  623.         MOVWF      R0+1
  624.         MOVF       _ut+2, 0
  625.         MOVWF      R0+2
  626.         MOVF       _ut+3, 0
  627.         MOVWF      R0+3
  628.         CALL       _double2byte+0
  629.         MOVF       R0+0, 0
  630.         MOVWF      FARG_PWM1_Set_Duty_new_duty+0
  631.         CALL       _PWM1_Set_Duty+0
  632. ;ENCODER.c,142 ::                 inttostr(setpoint,texto);
  633.         MOVF       _setpoint+0, 0
  634.         MOVWF      R0+0
  635.         MOVF       _setpoint+1, 0
  636.         MOVWF      R0+1
  637.         MOVF       _setpoint+2, 0
  638.         MOVWF      R0+2
  639.         MOVF       _setpoint+3, 0
  640.         MOVWF      R0+3
  641.         CALL       _double2int+0
  642.         MOVF       R0+0, 0
  643.         MOVWF      FARG_IntToStr_input+0
  644.         MOVF       R0+1, 0
  645.         MOVWF      FARG_IntToStr_input+1
  646.         MOVLW      _texto+0
  647.         MOVWF      FARG_IntToStr_output+0
  648.         CALL       _IntToStr+0
  649. ;ENCODER.c,144 ::                 lcd_Out(2,11,"       ");
  650.         MOVLW      2
  651.         MOVWF      FARG_Lcd_Out_row+0
  652.         MOVLW      11
  653.         MOVWF      FARG_Lcd_Out_column+0
  654.         MOVLW      ?lstr4_ENCODER+0
  655.         MOVWF      FARG_Lcd_Out_text+0
  656.         CALL       _Lcd_Out+0
  657. ;ENCODER.c,145 ::                 Lcd_Out(2,11,texto);
  658.         MOVLW      2
  659.         MOVWF      FARG_Lcd_Out_row+0
  660.         MOVLW      11
  661.         MOVWF      FARG_Lcd_Out_column+0
  662.         MOVLW      _texto+0
  663.         MOVWF      FARG_Lcd_Out_text+0
  664.         CALL       _Lcd_Out+0
  665. ;ENCODER.c,147 ::                 inttostr(pulsos,texto);
  666.         MOVF       _pulsos+0, 0
  667.         MOVWF      FARG_IntToStr_input+0
  668.         MOVF       _pulsos+1, 0
  669.         MOVWF      FARG_IntToStr_input+1
  670.         MOVLW      _texto+0
  671.         MOVWF      FARG_IntToStr_output+0
  672.         CALL       _IntToStr+0
  673. ;ENCODER.c,148 ::                 lcd_Out(4,8,"      ");
  674.         MOVLW      4
  675.         MOVWF      FARG_Lcd_Out_row+0
  676.         MOVLW      8
  677.         MOVWF      FARG_Lcd_Out_column+0
  678.         MOVLW      ?lstr5_ENCODER+0
  679.         MOVWF      FARG_Lcd_Out_text+0
  680.         CALL       _Lcd_Out+0
  681. ;ENCODER.c,149 ::                 lcd_Out(4,8,texto);
  682.         MOVLW      4
  683.         MOVWF      FARG_Lcd_Out_row+0
  684.         MOVLW      8
  685.         MOVWF      FARG_Lcd_Out_column+0
  686.         MOVLW      _texto+0
  687.         MOVWF      FARG_Lcd_Out_text+0
  688.         CALL       _Lcd_Out+0
  689. ;ENCODER.c,154 ::                 }
  690.         GOTO       L_main10
  691. ;ENCODER.c,158 ::                 }
  692. L_end_main:
  693.         GOTO       $+0
  694. ; end of _main
复制代码

所有资料51hei提供下载:
Encoder control de posicion motor.rar (90.61 KB, 下载次数: 10)






欢迎光临 (http://www.51hei.com/bbs/) Powered by Discuz! X3.1