roboforum.ru

А по чему в ПДФнике

А в чем надо? pdf - вещь универсальная.

почему не сразу на сайте?

На каком сайте?

Виталий писал(а):Ну вот вобщем начал потихонечку набрасывать.
http://quark-bot.blogspot.com/2007/11/blog-post.html

формулы пугають

Виталий писал(а):

А по чему в ПДФнике

А в чем надо? pdf - вещь универсальная.

почему не сразу на сайте?

На каком сайте?

Ну например на своем сайте и в html, но лучше здесь конечно...

Ну например на своем сайте и в html, но лучше здесь конечно...

Ну он типа и так на моем сайте. =)

формулы пугають

Словами объяснять лень =)

Виталий писал(а):

формулы пугають

Словами объяснять лень =)

Т.е. Си детям слабо, а вот такие формулы - запросто

Я щас тоже занят написанием программы для моего робота. Вообще, по поводу таймеров и счетчиков- надо считать скорость и частоту прихода импульсов. Вот у меня примерно так: 45 Гц ( скорость движения 30 см/с, 7,6 см длина окружности колеса, 10 прорезей), что значит примерно 22 мс. Но для нормального руления по прямой надо отслеживать разницу во времени на 1 сектор что-то около 100 мкс или меньше ( делал расчеты для базы в 10 см и радиуса поворота в 10 метров), то есть полюбому энкодеры надо вешать на прерывания. Далее, у меня включен нулевой таймер-счетчик, запоминает начальное значение по прерыванию от энкодера, и каждое переполнение таймера инкрементируется некий регистр ( я взял 2, с запасом). Затем, по следующему прерыванию от того же энкодера вычитаем из данного значения таймера запомненное при прошлом прерывании, и имеем в результате число в форме [число переполнений таймера (23...][значение тиков таймера (7..0)]. Таким образом я вырабатываю значения периода на 1 сектор для обоих колес, и сравниваю их в основной программе. Алгоритм довольно кондовый: сначала сравниваем старший байт, если один регистр времени больше, отнимаем, иначе прибавляем заполнение ШИМ. Ну и так по новой. То есть как таковой PID у меня нет. Сейчас пока отлаживаю тайминтг и прибавляемые значения, потом возьмусь за реальный PID ( для этого надо знать отклонение регулируемой величины от нужного значения, а самого нужного значения у меня нет, я не подбирал ШИМ для езды по прямой). Пишу все на асме, С хоть и учу, но мне он очень не нравится, да и тайминг у проги должен быть достаточно жестким когда повешу ИК бамперы. В принципе, ничего сложного я не изобретал. Представляю, какой лом разбираться в чужом асме, ак что попытаюсь накропать комментарии как можно скорее. Сорри за сумбурность, в универе постоянно спариваются с мозгами.

Код: Выделить всё • Развернуть

;*******************************************
; 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

up

ну так что, тема заглохла? а надо-бы сделать, надо... и для "интерпретатора", и в библиотеку.

у кого есть результаты?

У меня есть результаты на платформе RealRobot'a:
viewtopic.php?p=51555#p51555

Прошивка только не выложена... могу найти, выложить...

У меня есть результаты на платформе RealRobot'a:
viewtopic.php?p=51555#p51555

Прошивка только не выложена... могу найти, выложить...

обязательно! еще бы хорошо документашку, чтобы адаптировать для использования на различных платформах можно было..

=DeaD=, а =DeaD=... я напомнить просто.. ты не забыл?

Блин! И правда забыл! Сорри!

Правда я ща глянул и мне стыдно это выкладывать

Там я в процессе отладки из ПИД-алгоритма такое чудовище сделал своими тупыми подгонами IF операторами под то чего мне хочется

Вот в общем этот код в качестве примера жесткого изврата :

Код: Выделить всё • Развернуть

       //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;
        };

      };

Вот в общем этот код в качестве примера жесткого изврата :

большое списибо! будем разбирать.

Если вообще просто объяснить чего у меня там делается - у меня заведены переменные в которых я считаю путь пройденный каждым колесом и переменные в который такой же путь, только плановый. Затем на каждом шаге я смотрю какой ШИМ надо выдать, чтобы получить плановую скорость и если по пройденному пути перелёт уже - то в зависимости от перелёта снижаю ШИМ, и наоборот, если недолёт, то повышаю ШИМ. Короче симулянт я а начинал с правильного ПИД-регулятора

Короче симулянт я

в в общем выходит типа как здесь?

http://roboforum.ru/viewtopic.php?f=58&t=4392&p=61997#p61997

steel_monkey писал(а):Я щас тоже занят написанием программы для моего робота. Вообще, по поводу таймеров и счетчиков- надо считать скорость и частоту прихода импульсов. полюбому энкодеры надо вешать на прерывания.

Посомотрите апноут AN696 у http://MicroChip.com он и в примерах PROTEUS есть.

http://www.microchip.com/stellent/idcpl ... e=en011782