API tools faq
paste
Advertisement
Kelemvora

Untitled

Kelemvora
Nov 24th, 2019
162
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
ASM (NASM) 5.42 KB | None | 0 0
raw download clone embed print report
  1. .equ    TH          =0b11000011 ;50000
  2. .equ    TL          =0b01010000
  3. .def  tmp0      =r16
  4. .def    tmp1        =r17
  5. .def    tmp2        =r18
  6. .def    tcnt        =r19
  7. .def    dcnt        =r20
  8. .def    adccnt  =r21
  9. .def    res0        =r22
  10. .def    res1        =r23
  11. .def    res2        =r24
  12. .def    res3        =r25
  13.  
  14. .dseg
  15. InWave:
  16.     .byte 1
  17. Kp:
  18.     .byte 1
  19. Ki:
  20.     .byte 1
  21. Kd:
  22.     .byte 1
  23. Error:
  24.     .byte 1
  25. Error_1:
  26.     .byte 1
  27. Error_2:
  28.     .byte 1
  29. ErrSum:
  30.     .byte 2
  31. IntRes:
  32.     .byte 2
  33. DerRes:
  34.     .byte 2
  35.  
  36. .cseg
  37. .org 0x0000
  38. rjmp Init
  39. .org 0x0006
  40. rjmp TInit
  41. .org 0x001C
  42. rjmp ADC_Complete
  43.  
  44. Init:
  45.     ldi tmp0, LOW(RAMEND)
  46.    out SPL, tmp0
  47.    ldi tmp0, HIGH(RAMEND)
  48.    out SPH, tmp0
  49.  
  50.    sei
  51.    
  52.     ldi YL, low(Kp)
  53.     ldi YH, high(Kp)
  54.     clr tmp0
  55.     st Y, tmp0
  56.     ldi YL, low(Ki)
  57.     ldi YH, high(Ki)
  58.     st Y, tmp0
  59.     ldi YL, low(Kd)
  60.     ldi YH, high(Kd)
  61.     st Y, tmp0
  62.     ldi YL, low(Error)
  63.     ldi YH, high(Error)
  64.     st Y, tmp0
  65.     ldi YL, low(Error_1)
  66.     ldi YH, high(Error_1)
  67.     st Y, tmp0
  68.     ldi YL, low(Error_2)
  69.     ldi YH, high(Error_2)
  70.     st Y, tmp0
  71.     ldi YL, low(ErrSum)
  72.     ldi YH, high(ErrSum)
  73.     st Y+, tmp0
  74.     st Y, tmp0
  75.    
  76.    out DDRA, tmp0
  77.    ldi tmp0, (1 << DDA1)|(1 << DDA2)|(1 << DDA3)
  78.    out PORTA, tmp0
  79.  
  80.     ldi tmp0, 0xFF
  81.    out DDRB, tmp0
  82.    clr tmp0
  83.    out PORTB, tmp0
  84.    
  85.     clr adccnt
  86.     ldi tmp0, (1<<REFS0)|(1<<ADLAR)|(0<<MUX2)|(0<<MUX1)|(0<<MUX0)
  87.     out ADMUX, tmp0
  88.     ldi tmp0, (1<<ADSC)|(1<<ADFR)|(1<<ADIE)|(0<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)
  89.     out ADCSR, tmp0
  90.    
  91.     clr tcnt
  92.     ldi tmp0, 10
  93.     out OCR2, tmp0
  94.     ldi tmp0, (1<<WGM21)|(1<<CS22)|(1<<CS20)
  95.     out TCCR2, tmp0
  96.     ldi tmp0, (1<<OCIE2)
  97.     out TIMSK, tmp0
  98.    
  99.    rjmp Start
  100.  
  101. TInit:
  102.     inc tcnt
  103.    cpi tcnt, 5
  104.    brne TExit
  105.    clr tcnt
  106.    clr adccnt
  107.    cbi ADMUX, MUX1
  108.    cbi ADMUX, MUX0
  109. TExit:
  110.     sbi ADCSR, ADEN
  111.     reti
  112.  
  113. ADC_Complete:
  114.     cbi ADCSR, ADEN
  115.     cpi adccnt, 0
  116.     breq Input
  117.     cpi adccnt, 1
  118.     breq Proportional
  119.     cpi adccnt, 2
  120.     breq Integral
  121.     cpi adccnt, 3
  122.     breq Derivative
  123.     reti
  124.  
  125. Input:
  126.     inc adccnt 
  127.     ldi YL, low(InWave)
  128.     ldi YH, high(InWave)
  129.     in tmp0, ADCH
  130.     st Y, tmp0
  131.     cbi ADMUX, MUX1
  132.     sbi ADMUX, MUX0
  133.     sbi ADCSR, ADEN
  134.     reti
  135.  
  136. Proportional:
  137.     inc adccnt
  138.     ldi YL, low(Kp)
  139.     ldi YH, high(Kp)
  140.     in tmp0, ADCH
  141.     lsr tmp0
  142.     lsr tmp0
  143.     lsr tmp0
  144.     lsr tmp0
  145.     st Y, tmp0
  146.     sbi ADMUX, MUX1
  147.     cbi ADMUX, MUX0
  148.     sbi ADCSR, ADEN
  149.     reti
  150.    
  151. Integral:
  152.     inc adccnt
  153.     ldi YL, low(Ki)
  154.     ldi YH, high(Ki)
  155.     in tmp0, ADCH
  156.     lsr tmp0
  157.     lsr tmp0
  158.     lsr tmp0
  159.     lsr tmp0
  160.     st Y, tmp0
  161.     sbi ADMUX, MUX1
  162.     sbi ADMUX, MUX0
  163.     sbi ADCSR, ADEN
  164.     reti
  165.  
  166. Derivative:
  167.     ldi adccnt, 1
  168.     ldi YL, low(Kd)
  169.     ldi YH, high(Kd)
  170.     in tmp0, ADCH
  171.     lsr tmp0
  172.     lsr tmp0
  173.     lsr tmp0
  174.     lsr tmp0
  175.     st Y, tmp0
  176.     cbi ADMUX, MUX1
  177.     sbi ADMUX, MUX0
  178.     rjmp Output
  179.  
  180. Output:
  181. ;Error
  182.     ldi YL, low(Error_1)
  183.     ldi YH, high(Error_1)
  184.     ld tmp0, Y
  185.     ldi ZL, low(Error_2)
  186.     ldi ZH, high(Error_2)
  187.     st Z, tmp0
  188.     ldi XL, low(Error)
  189.     ldi XH, high(Error)
  190.     ld tmp0, X
  191.     st Y, tmp0
  192.     ldi ZL, low(InWave)
  193.     ldi ZH, high(InWave)
  194.     ld tmp1, Z
  195.     in tmp2, PORTB
  196.     sub tmp1, tmp2
  197.     brcc PosErr
  198.     ldi tmp2, 0xFF
  199.     rjmp PC+2
  200. PosErr:
  201.     clr tmp2
  202.     st X, tmp1
  203.     ldi ZL, low(ErrSum+1)
  204.     ldi ZH, high(ErrSum+1)
  205.     ld tmp0, Z
  206.     add tmp0, tmp1
  207.     st Z, tmp0
  208.     ldi YL, low(ErrSum)
  209.     ldi YH, high(ErrSum)
  210.     ld tmp0, Y
  211.     adc tmp0, tmp2
  212.     st Y, tmp0
  213.    
  214. ; Integral
  215.     ldi ZL, low(Ki)
  216.     ldi ZH, high(Ki)
  217.     ld tmp0, Z
  218.     ld tmp1, Y
  219.     mul tmp0, tmp1
  220.     mov res0, r0
  221.     mov res1, r1   
  222.     ldi tmp0, 3
  223.     mul tmp0, res1
  224.     mov res1, r0
  225.     mov res2, r1
  226.     mul tmp0, res0
  227.     clr tmp0
  228.     mov res0, r0
  229.     add res1, r1
  230.     adc res2, tmp0
  231.     ldi dcnt, 251
  232.     clr r0
  233.     clr r1
  234. DecInt:
  235.     clr tmp1
  236.     ldi tmp0, 10
  237.     sub res2, tmp0
  238.     inc tmp1
  239.     brcc PC-2
  240.     add res2, tmp0
  241.     dec tmp1
  242.     inc dcnt
  243.     breq DecIntEx
  244.     add r0, tmp1
  245.     clr tmp1
  246.     adc r1, tmp1
  247.     lsl r0
  248.     rol r1
  249.     lsl r0
  250.     rol r1
  251.     lsl r0
  252.     rol r1
  253.     lsl r0
  254.     rol r1
  255.     lsl res0
  256.     rol res1
  257.     rol res2
  258.     lsl res0
  259.     rol res1
  260.     rol res2
  261.     lsl res0
  262.     rol res1
  263.     rol res2
  264.     lsl res0
  265.     rol res1
  266.     rol res2
  267.     rjmp DecInt
  268. DecIntEx:
  269.     add r0, tmp1
  270.     clr tmp1
  271.     adc r1, tmp1
  272.     ldi ZL, low(IntRes)
  273.     ldi ZH, high(IntRes)
  274.     st Z+, r1
  275.     st Z, r0   
  276.     ldi ZL, low(IntRes)
  277.     ldi ZH, high(IntRes)
  278.     st Z+, r1
  279.     st Z, r0
  280.    
  281. ; Derivativ
  282.     ldi ZL, low(Error)
  283.     ldi ZH, high(Error)
  284.     ld tmp0, Z
  285.     ldi ZL, low(Error_2)
  286.     ldi ZH, high(Error_2)
  287.     ld tmp1, Z
  288.     sub tmp0, tmp1
  289.     ldi ZL, low(Kd)
  290.     ldi ZH, high(Kd)
  291.     ld tmp1, Z
  292.     mul tmp1, tmp0
  293.     mov r2, r0
  294.     mov r3, r1
  295.     ldi tmp0, TL
  296.     ldi tmp1, TH
  297.     mul tmp0, r2
  298.     mov res0, r0
  299.     mov res1, r1
  300.     mul tmp1, r3
  301.     mov res2, r0
  302.     mov res3, r1
  303.     mul tmp0, r3
  304.     clr tmp0
  305.     add res1, r0
  306.     adc res2, r1
  307.     adc res3, tmp0
  308.     mul tmp1, r2
  309.     add res1, r0
  310.     adc res2, r1
  311.     adc res3, tmp0 
  312.     lsl res0
  313.     rol res1
  314.     rol res2
  315.     rol res3
  316.     ldi YL, low(DerRes)
  317.     ldi YH, high(DerRes)
  318.     st Y+, res2
  319.     st Y, res1
  320.    
  321. ; Proportional 
  322.     ldi YL, low(Error)
  323.     ldi YH, high(Error)
  324.     ldi ZL, low(Kp)
  325.     ldi ZH, high(Kp)
  326.     ld tmp0, Y
  327.     ld tmp1, Z
  328.     mul tmp0, tmp1
  329.     mov tmp0, r0   
  330.     mov tmp1, r1
  331.    
  332. ; Result
  333.     ldi YL, low(DerRes)
  334.     ldi YH, high(DerRes)
  335.     ld res1, Y+
  336.     ld res0, Y
  337.     ldi YL, low(IntRes)
  338.     ldi YH, high(IntRes)
  339.     ld res3, Y+
  340.     ld res2, Y
  341.     add res0, res2
  342.     adc res1, res3
  343.     add res0, tmp0
  344.     adc res1, tmp1  
  345.     ldi dcnt, 253
  346.     clr r0
  347.     clr r1
  348. DecOut:
  349.     clr tmp1
  350.     ldi tmp0, 10
  351.     sub res1, tmp0
  352.     inc tmp1
  353.     brcc PC-2
  354.     add res1, tmp0
  355.     dec tmp1
  356.     inc dcnt
  357.     breq DecOutEx
  358.     add r0, tmp1
  359.     clr tmp1
  360.     adc r1, tmp1
  361.     lsl r0
  362.     rol r1
  363.     lsl r0
  364.     rol r1
  365.     lsl r0
  366.     rol r1
  367.     lsl r0
  368.     rol r1
  369.     lsl res0
  370.     rol res1
  371.     lsl res0
  372.     rol res1
  373.     lsl res0
  374.     rol res1
  375.     lsl res0
  376.     rol res1
  377.     rjmp DecOut
  378. DecOutEx:
  379.     add r0, tmp1
  380.     clr tmp1
  381.     adc r1, tmp1   
  382.     out PORTB, r0      
  383.     reti
  384.  
  385. Start:
  386.     rjmp Start
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!