roboforum.ru

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

   
   
   
   
     

  };
}