roboforum.ru

;*******************************************
; Author                                   *
; Date                         07.10.2007  *
; Version                      1.00        *
; Filename                     robot       *
; Device                       atmega8L    *
; Frequency                    8 MHz       *
;*******************************************



.device atmega8
.nolist
.include "C:\Program Files\Atmel\AVR Tools\AvrAssembler2\Appnotes\m8def.inc"
.list

;===========================================
;Declarations

.def EEreg=R3
.def Temp=R16
.def Temp2=R17
.def Temp3=R18
.def Temp4=R19
.def Temp_SP1=R20
.def temp_SP2=R21
.def int_reg=R2
.def left_motor_low=R22
.def left_motor_high=R23
.def right_motor_low=R24
.def right_motor_high=R25




   ;              |-------- ext interrupt 0:  interrupt had passed 4 times
   ;              ||------- ext interrupt 1:  interrupt had passed 4 times
   ;              |||------
   ;              ||||-----
   ;              |||||----
   ;              ||||||---right motor enable         
   ;              |||||||--left motor enable
   ;              ||||||||-
;.def flag=R19;    0bxxxxxxxx



; EQUs
.equ forward_msk=0b10010000 ; PORTD mask for 4 left bytes
.equ backward_msk=0b01100000


; SRAM BEGIN: 0x0060
; SRAM descripting
.equ diff_hi=20


.equ num_ext_int_0=0x0060
.equ num_ext_int_1=0x0061
.equ timer_interrupt_0_low=0x0062
.equ timer_interrupt_0_high=0x0063
.equ timer_interrupt_1_low=0x0064
.equ timer_interrupt_1_high=0x0065
.equ timer_0_initial_0=0x0066
.equ timer_0_initial_1=0x0068
.equ value_0_b2=0x0067
.equ value_0_b1=0x0069
.equ value_0_b0=0x0070
.equ value_1_b2=0x0071
.equ value_1_b1=0x0072
.equ value_1_b0=0x0073
.equ e_counter_low=0x0074
.equ e_counter_high=0x0075
.equ timer_interrupt_0_high_2=0x0076
.equ timer_interrupt_1_high_2=0x0077
.equ value_0_b3=0x0078
.equ value_1_b3=0x0079
.equ counter_low=0x0080
.equ counter_high=0x0081

.cseg
.org   0


rjmp   RESET     
rjmp   INT_0     
rjmp   INT_1     
rjmp   TC2_comp_mch     
rjmp   TC2_ovf
rjmp   TC1_capt     
rjmp   TC1_comp_A     
rjmp   TC1_comp_B     
rjmp   TC1_ovf   
rjmp   TC0_ovf     
rjmp   SPI_STC     
rjmp   USART_RXC     
rjmp   USART_UDRE 
rjmp   USART_TXC     
rjmp   ADC_C     
rjmp   EE_RDY     
rjmp   ANA_CMP     
rjmp   TWI     
rjmp   SMP_RDY   
       
         
         
 
  TC2_comp_mch:
  TC2_ovf:
  TC1_capt:     
  TC1_comp_A:     
  TC1_comp_B:     
  TC1_ovf:   
       
  SPI_STC:     
  USART_RXC:     
  USART_UDRE: 
  USART_TXC:     
  ADC_C:     
  EE_RDY:     
  ANA_CMP:     
  TWI:     
  SMP_RDY:
    reti   





; Interuupt 0 handler.
INT_0:
push temp_SP1
push temp
in temp,sreg
push temp




in temp, TCNT0
lds temp2, timer_0_initial_0
lds temp3, timer_interrupt_0_low
lds temp4, timer_interrupt_0_high
lds temp_SP1, timer_interrupt_0_high_2
sub temp2, temp
sbci temp3, 0
sbci temp4, 0
sbci temp_SP1, 0
sts timer_0_initial_0, temp
sts value_0_b0, temp2
sts value_0_b1, temp3
sts value_0_b2, temp4
sts value_0_b3, temp_SP1
clr temp
sts timer_interrupt_0_low, temp
sts timer_interrupt_0_high, temp
sts timer_interrupt_0_high_2, temp




; ldi temp, 0b00000010
; or int_reg, temp



pop temp
out Sreg, temp
pop temp
pop temp_SP1
reti





       
INT_1:   
push temp_sp1
push temp
in temp,sreg
push temp

in temp, TCNT0
lds temp2, timer_0_initial_1
lds temp3, timer_interrupt_1_low
lds temp4, timer_interrupt_1_high
lds temp_SP1, timer_interrupt_1_high_2
sub temp2, temp
sbci temp3, 0
sbci temp4, 0
sbci temp_SP1, 0
sts timer_0_initial_1, temp
sts value_1_b0, temp2
sts value_1_b1, temp3
sts value_1_b2, temp4
sts value_1_b3, temp_SP1
clr temp
sts timer_interrupt_1_low, temp
sts timer_interrupt_1_high, temp
sts timer_interrupt_1_high_2, temp

; ldi temp, 0b00000010
; or int_reg, temp

pop temp
out Sreg, temp
pop temp
pop temp_sp1

reti







TC0_ovf:
push temp
in temp,sreg
push temp
lds temp2, timer_interrupt_0_low
lds temp3, timer_interrupt_0_high
;lds temp4, timer_interrupt_0_high_2
ldi temp, 1
add temp2, temp
clr temp
adc temp3, temp
;adc temp4, temp




;cpi temp4, 255
;brne mem_store1            ; RIGHT MOTOR
cpi temp3, 9
brne mem_store1
cpi temp2, 255
brne mem_store1
ldi temp2, 254
ldi temp3, 8
sbi PORTD, 0


;ldi  temp, 100
;add left_motor_low, temp
;clr temp
;adc left_motor_high, temp
;out  OCR1AH, left_motor_high           ; left motor high   
;out  OCR1AL, left_motor_low   

mem_store1:
sts timer_interrupt_0_low, temp2
sts timer_interrupt_0_high,temp3
;sts timer_interrupt_0_high_2, temp4

lds temp2, timer_interrupt_1_low
lds temp3, timer_interrupt_1_high
;lds temp4, timer_interrupt_1_high_2
ldi temp, 1
add temp2, temp
clr temp
adc temp3, temp
;adc temp4, temp




;cpi temp4, 255
;brne mem_store2        ;LEFT MOTOR
cpi temp3, 4
brlo mem_store2
ldi temp3, 2
ldi temp, 150
add left_motor_low, temp
clr temp
adc left_motor_high, temp
sbi PORTD, 0
;ldi  temp, 100
;add left_motor_low, temp
;clr temp
;adc left_motor_high, temp
;out  OCR1AH, left_motor_high           ; left motor high   
;out  OCR1AL, left_motor_low 

mem_store2:
sts timer_interrupt_1_low, temp2
sts timer_interrupt_1_high,temp3
;sts timer_interrupt_1_high_2,temp4


lds temp, counter_low
inc temp

cpi temp, 30
brne out_
clr temp
ldi temp2, 0b00000010
or int_reg, temp2


out_:
sts counter_low, temp
pop temp
out Sreg, temp
pop temp
reti





         
RESET:   

;*********************************************
;Stack init and analog comparator turning off
;*********************************************
   ldi    temp, high(ramend)   ;stack init
   out    SPH, temp   
   ldi    temp, low(ramend)   
   out    SPL, temp   
   ldi temp, 0b10000000
   out ACSR, temp

;*************************
;port setup
;*************************


;            |--------right motor
;            ||-------right motor
;            |||------left motor
;            ||||-----left motor
;            |||||----
;            ||||||---         
;            |||||||--LED
;            ||||||||-LED
ldi temp, 0b11110011
out DDRD, temp
ldi temp, 0b10010010
out PORTD, temp




;            |--------
;            ||-------
;            |||------
;            ||||-----
;            |||||----
;            ||||||---right motor enable         
;            |||||||--left motor enable
;            ||||||||-
ldi temp, 0b00000110
out DDRB, temp
ldi temp, 0b00000000
out PORTB, temp
         
;*************************
;Initial motor PWM values
;*************************
ldi left_motor_low,  low(700)
ldi left_motor_high, high(700)
ldi right_motor_low,  low(900)
ldi right_motor_high, high(900)

;*************************
; Timer 1 setup
;*************************
ldi temp, 0x12
  out TCCR1B, temp
ldi temp, 0xF0
out TCCR1A, temp
ldi temp, HIGH(1023)
out ICR1H, temp
ldi temp, LOW(1023)
out ICR1L, temp                 
out  OCR1AH, left_motor_high           ; left motor high   
out  OCR1AL, left_motor_low            ; left motor low
out  OCR1BH, right_motor_high          ; right motor high                 
out  OCR1BL, right_motor_low           ; right motor low


;*************************
; Timer 0 setup
;*************************
ldi temp, 0x02
out TCCR0, temp
ldi temp, (1<<(TOIE0))
out timsk, temp



;***************************
; DEBUG ONLY
;***************************

;***************************
; PORTC SETUP
ldi temp, 0b00111111
out DDRC, temp
;***************************

;***************************
; SRAM cleaning
ldi temp, 0b00000000
sts num_ext_int_0,temp
sts num_ext_int_1, temp
sts value_0_b3, temp
sts value_1_b3, temp
sts timer_interrupt_0_high_2, temp
sts timer_interrupt_1_high_2, temp
sts timer_interrupt_0_low, temp
sts timer_interrupt_0_high, temp
sts timer_interrupt_1_low, temp
sts timer_interrupt_1_high, temp
sts timer_0_initial_0, temp
sts timer_0_initial_1, temp
sts value_0_b2, temp
sts value_0_b1, temp
sts value_0_b0, temp
sts value_1_b2, temp
sts value_1_b1, temp
sts value_1_b0, temp
sts e_counter_low, temp
sts e_counter_high, temp
;**************************

;*************************
; External interrupt setup
;*************************
in temp, MCUCR
ori temp, 0b00001111
out MCUCR, temp

in temp, GICR
ori temp, 0b11000000
out GICR, temp
clr TEMP3
sei







Main:
;**************************
;START DEBUG
; lds temp, value_0_b2
; out portC, temp
;STIOP DEBUG
;**************************
sbrs int_reg, 1

rjmp main


lds temp_SP1, value_0_b2
lds temp_SP2, value_1_b2
cp temp_SP1, temp_SP2
breq Ravno_b2
brlo I_0_less_1_b2



I_1_less_0_b2:

ldi temp, diff_hi
sub left_motor_low, temp
sbci left_motor_high, 0
cpi left_motor_low, low(diff_hi)
brlo up_1
rjmp out_1
up_1:
cpi left_motor_high,0
breq up_2
rjmp out_2
up_2:
ldi left_motor_low, low(diff_hi)
ldi left_motor_high, high(diff_hi)
out_2:
out  OCR1AH, left_motor_high           ; left motor high   
out  OCR1AL, left_motor_low            ; left motor low
ldi temp, 0b11111101
and int_reg, temp
rjmp main




I_0_less_1_b2:




ldi temp, diff_hi
clr temp_SP1
add left_motor_low, temp
adc left_motor_high, temp_SP1
cpi left_motor_high, high (1023-diff_hi)
breq down_1
rjmp out_1
down_1:
cpi left_motor_low, low(1023-diff_hi)
brsh down_2
rjmp out_1
down_2:
ldi left_motor_low, low(1032-diff_hi-1)
ldi left_motor_high, high(1032-diff_hi-1)
out_1:
out  OCR1AH, left_motor_high           ; left motor high   
out  OCR1AL, left_motor_low            ; left motor low
ldi temp, 0b11111101
and int_reg, temp
rjmp main


Ravno_b2:
lds temp_SP1, value_0_b1
lds temp_SP2, value_1_b1

cp temp_SP1, temp_SP2
breq Ravno_b1
brlo I_0_less_1_b1

I_1_less_0_b1:
ldi temp, 3
sub left_motor_low, temp
sbci left_motor_high, 0

cpi left_motor_low, low(40)
brlo up_3
rjmp out_4
up_3:
cpi left_motor_high,0
breq up_4
rjmp out_4
up_4:
ldi left_motor_low, low(40)
ldi left_motor_high, high(40)
out_4:

out  OCR1AH, left_motor_high           ; left motor high   
out  OCR1AL, left_motor_low            ; left motor low
ldi temp, 0b11111101
and int_reg, temp
rjmp main


I_0_less_1_b1:






ldi temp, 3
add left_motor_low, temp
clr temp
adc left_motor_high, temp
cpi left_motor_high, high (1018)
breq down_3
rjmp out_3
down_3:
cpi left_motor_low, low(1018)
brsh down_4
rjmp out_3
down_4:
ldi left_motor_low, low(1017)
ldi left_motor_high, high(1017)
out_3:
out  OCR1AH, left_motor_high           ; left motor high   
out  OCR1AL, left_motor_low            ; left motor low
ldi temp, 0b11111101
and int_reg, temp
rjmp main

Ravno_b1:
ldi temp, 0b11111101
and int_reg, temp
rjmp main





;*********************
; EEPROM programming, EEreg contains information to be written
;*********************

EEPROM_writing:
wait_write:
; sbic EECR, EEWE
; rjmp wait_write


; clr temp
; out EEARH, temp
; ldi temp, 1
; out EEARL, temp


; cli

; ldi temp, 120
; out EEDR, temp
; sbi EECR, EEMWE
;sbi EECR, EEWE

;*********************
; ldi temp, 0b00000001
; or int_reg, temp

;!!!!!!!
; sei
;!!!!!!!
; rjmp main

       //printf("+");
      
      //Do motor control 50 times per second
      
      if( queue_size>0 ){

        int32_t d,x,xx,y,t0,sp;

        if( current_timer == time[0] ){

         t0=time[0];
         sp=speed2s[0];
          d=speed2f[0]; d-=speed2s[0];
         x=current_timer; x*=d; x/=t0; x+=sp; x=200*x/250;
         y=current_timer; y=maxspeed*y*y*d/t0/0x7F/2+maxspeed*y*sp/0x7F+startRight;
         startRight=y;

         sp=speed1s[0];
          d=speed1f[0]; d-=speed1s[0];
         x=current_timer; x*=d; x/=t0; x+=sp; x=200*x/250;
         y=current_timer; y=maxspeed*y*y*d/t0/0x7F/2+maxspeed*y*sp/0x7F+startLeft;
         startLeft=y;

         //printf("SpeedL=%d\n",10*cnt_left/time[0]);
         //printf("SpeedR=%d\n",10*cnt_right/time[0]);
          queue_size--;
         int i;
         for(i=0; i<queue_size; i++){
           speed1s[i]=speed1s[i+1];
           speed1f[i]=speed1f[i+1];
           speed2s[i]=speed2s[i+1];
           speed2f[i]=speed2f[i+1];
           time[i]=time[i+1];
         };
         current_timer=0;
        };

        if( queue_size>0 ){

         //Calculate current speed of right motor and set PWM and direction pins

         t0=time[0];
         sp=speed2s[0];

         //printf("t=%d\n",current_timer);

          d=speed2f[0]; d-=speed2s[0];
         x=current_timer; x*=d; x/=t0; x+=sp; x=200*x/250; xx=x;
         y=current_timer; y=maxspeed*y*y*d/t0/0x7F/2+maxspeed*y*sp/0x7F+startRight;
         
         if(cnt_right>y+1){ x=x-10; };
         if(cnt_right>y+2){ x=x-20; };
         if(x-30>-127)if(cnt_right>y+3){ x=x-30; };
         if(x-40>-127)if(cnt_right>y+4){ x=x-40; };
         if(cnt_right<y-1){ x=x+10; };
         if(cnt_right<y-2){ x=x+20; };
         if(x+30<127)if(cnt_right<y-3){ x=x+30; };
         if(x+40<127)if(cnt_right<y-4){ x=x+40; };
         
         
         //printf("r=%ld x=%ld dx=%ld y=%ld\n",cnt_right,x,x-xx,y);
         if(x>0){
             value[0x12]=1;
             outb(PORTD,inb(PORTD) & 0xDF); //Set PD5=0
            uint8_t speed=x;
            speed+=0x60;
            OCR3AH=0x00;
               OCR3AL=speed;
         }else if(x<0){
             value[0x12]=2;
             outb(PORTD,inb(PORTD) | 0x20); //Set PD5=1
            uint8_t speed=-x;
            speed+=0x60;
            OCR3AH=0x00;
               OCR3AL=speed;
         }else{
            uint8_t speed=0;
            OCR3AH=0x00;
               OCR3AL=speed;
         };

         //Calculate current speed of left motor and set PWM and direction pins

         sp=speed1s[0];

          d=speed1f[0]; d-=speed1s[0];
         x=current_timer; x*=d; x/=t0; x+=sp; x=200*x/250; xx=x;
         y=current_timer; y=maxspeed*y*y*d/t0/0x7F/2+maxspeed*y*sp/0x7F+startLeft;
         
         
         if(cnt_left>y+1){ x=x-10; };
         if(cnt_left>y+2){ x=x-20; };
         if(x-30>-127)if(cnt_left>y+3){ x=x-30; };
         if(x-40>-127)if(cnt_left>y+4){ x=x-40; };
         if(cnt_left<y-1){ x=x+10; };
         if(cnt_left<y-2){ x=x+20; };
         if(x+30<127)if(cnt_left<y-3){ x=x+30; };
         if(x+40<127)if(cnt_left<y-4){ x=x+40; };
         
         
         //printf("l=%ld x=%ld dx=%ld y=%ld\n",cnt_left,x,x-xx,y);
         if(x>0){
             value[0x10]=1;
            outb(PORTD,inb(PORTD) & 0xEF); //Set PD4=0 LeftDrive - Forward
            uint8_t speed=x;
            speed+=0x60;
            OCR3BH=0x00;
               OCR3BL=speed;
         }else if(x<0){
             value[0x10]=2;
            outb(PORTD,inb(PORTD) | 0x10); //Set PD4=1 LeftDrive - Backward
            uint8_t speed=-x;
            speed+=0x60;
            OCR3BH=0x00;
               OCR3BL=speed;
         }else{
            uint8_t speed=0;
            OCR3BH=0x00;
               OCR3BL=speed;
         };

         current_timer++;
        }else{
          OCR3BH=0x00;
            OCR3BL=0x00;
         OCR3AH=0x00;
            OCR3AL=0x00;
        };

      };