roboforum.ru

**MaPTbIH** » 26 апр 2012, 20:10

Код: Выделить всё: //by MaPTbIH-inc //OR-LT-v2 //LT_LineTestMotor 05.04.12 #include "defines.h" #include <ctype.h> #include <stdint.h> #include <stdio.h> #include <string.h> #include <stdarg.h> #include <avr/interrupt.h> #include <util/delay.h> #include "uart.c" #define PAUSEMOTORCHANGE 25 FILE uart_str = FDEV_SETUP_STREAM(uart_putchar, uart_getchar, _FDEV_SETUP_RW); uint16_t baseA; uint16_t baseB; void WriteFormatted (char * format, ...) { va_list args; va_start (args, format); vprintf (format, args); va_end (args); } int PowerBase (int Power) { int tmp=0; if (Power<0) { tmp=260+Power*0.9; } if (Power==0) { tmp=0; } if (Power>0) { tmp=260+Power*0.9; } return tmp; } void Turn(int DegreeOfTurn, int Power) {//DegreeOfTurn==0 Прямj // -100 -> налево вокруг себя, -50 -> налево // +100 -> направо вокруг себя, +50 -> направо int A,B; int Kslow=100/Power; B=Power; A=1.0*Power; if (DegreeOfTurn==0) {baseA=PowerBase(A);baseB=PowerBase(B);} if (DegreeOfTurn<0 && DegreeOfTurn>=-50) {baseA=PowerBase( (DegreeOfTurn+50)*1.8/Kslow );baseB=PowerBase(B);} if (DegreeOfTurn<-50 && DegreeOfTurn>=-100) {baseA=PowerBase( (DegreeOfTurn+50)*1.8/Kslow );baseB=PowerBase(B);} if (DegreeOfTurn<50 && DegreeOfTurn>0) {baseA=PowerBase(A);baseB=PowerBase( (DegreeOfTurn-50)*(-2.0)/Kslow );} if (DegreeOfTurn<=100 && DegreeOfTurn>=50) {baseA=PowerBase(A);baseB=PowerBase( (DegreeOfTurn-50)*(-2.0)/Kslow );} //printf("Turn=%d PowerA=%d PowerB=%d\n",DegreeOfTurn,baseA,baseB); _delay_ms(PAUSEMOTORCHANGE); OCR1A=baseA; OCR1B=baseB; } void Stop() { baseA=PowerBase(0); baseB=PowerBase(0); _delay_ms(PAUSEMOTORCHANGE); OCR1A=baseA; OCR1B=baseB; } int main(void) { uart_init(); stdout = stdin = &uart_str; int mid=5,Error=0,Derivative=0,Integral=0,PrevError=0,Timer=0; int Ki=90; int Kp=1,Kd=70,PowerMotors=100; printf("Linetracer ready 1!\n"); DDRC=0x20; DDRD=0x30; Stop(); _delay_ms(100); //PWM, Phase and frequency correct, TOP=ICR1 TCCR1A=0xA0; //clear on up-counting, set on down-counting TCCR1B=0x12; //Clock prescaler 1/8 ICR1=9216; //Set 50Hz frequency Stop(); while(1==1){ DDRA=0xFF; PORTA=0xFF; DDRC|=0xC0; PORTC|=0xC0; _delay_us(1000); DDRA=0x00; PORTA=0x00; DDRC&=~0xC0; PORTC&=~0xC0; PORTC|=0x20; _delay_us(500); int x=PINA; int y=PINC>>6; if(y==1){ y=2; }else if (y==2){y=1;}; x=x+(y<<8); int pos1=-1; int pos2=10; for(int i=0; i<10; i++){ int j=x&0x01; if(j==1 && pos1==-1){ pos1=i; }; if(j==1){ pos2=i; }; //printf("%d",j); x=x>>1; }; int pos=(pos2+pos1)/2; //PID-controller int DOT=0; switch (pos){ case 9: DOT=-50;PowerMotors=15; break; case 8: DOT=-40;PowerMotors=50; break; case 7: DOT=-30;PowerMotors=85; break; case 6: DOT=-20;PowerMotors=90; break; case 5: DOT=-10;PowerMotors=100; break; case 4: DOT=10;PowerMotors=100; break; case 3: DOT=20;PowerMotors=90; break; case 2: DOT=30;PowerMotors=85; break; case 1: DOT=40;PowerMotors=50; break; case 0: DOT=50;PowerMotors=15; break; default: break;} Error=mid-pos; Derivative=Error-PrevError; Integral+=Error; DOT=DOT+Kd*Derivative+Integral/Ki; PrevError=Error; if (DOT<=-100) DOT=-100; if (DOT>=100) DOT=100; Turn(DOT,PowerMotors); _delay_us(1000); }; }

roboforum.ru

OR-LT-v2 (cover)

Re: OR-LT-v2 (cover)

Кто сейчас на конференции