//Robot Zero en PCB
#define F_CPU 20000000UL // Baby Orangutan frequency (20MHz)
#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
//Leds. Salidas.
#define LEDP PORTD1
#define LEDV PORTB1
#define LEDA PORTB5
#define LEDR PORTB4
//Interruptores. Entradas.
#define SW1 PORTB0
#define SW2 PORTB2
//Sensores. Entradas y salidas.
#define D3 PORTD7
#define D2 PORTD4
#define D1 PORTD2
#define D0 PORTD0
#define I0 PORTC3
#define I1 PORTC2
#define I2 PORTC1
#define I3 PORTC0
void inicializar_puertos(void);
void reset(void);
void M1_forward(unsigned char pwm);
void M1_reverse(unsigned char pwm);
void M2_forward(unsigned char pwm);
void M2_reverse(unsigned char pwm);
void motors_init(void);
int obtener_errorp(void);
void inicializar_timer1(void);
int obtener_errord(void);
/*********** Ajuste comportamiento robot, motores HP *********/
//Constantes Regulador PD. 7 800-1600 95, 7 1200 95, este 7 1400 110, 7 1600 110
int Kp=7; //9
int Kd=1450;//1400 110
volatile int velocidad = 110; //Máxima 255.
/*************************************************/
int main( void )
{
// int errorp = 0;
// char leer_pin1 = 0;
// char leer_pin2 = 0;
inicializar_puertos();
motors_init();
reset();
inicializar_timer1();
M1_forward(0); //Motor derecho.
M2_forward(0); //Motor izquierdo.
/* leer_pin1 = PINB & (1<<SW1);
if(leer_pin1 != 0)
{
leer_pin2 = PINB & (1<<SW2);
if(leer_pin2 != 0)
{
velocidad = 255;
}
else if(leer_pin2 == 0)
{
velocidad = 175;
}
}
else if(leer_pin1 == 0)
{
leer_pin2 = PINB & (1<<SW2);
if(leer_pin2 != 0)
{
velocidad = 125;
}
else if(leer_pin2 == 0)
{
velocidad = 75;
}
}*/
while ( 1 )
{
}
return 0;
}
void inicializar_puertos(void)
{
DDRD=0x68; //0110 1000
PORTD=0x00;
DDRB=0x3A; //0011 1010
PORTB=0x00;
DDRC=0x00; //0000 0000
PORTC=0x00;
}
void reset(void)
{
PORTD |= (1<<LEDP); //Encendemos Led en Baby Orangutan.
PORTB &= ~(1<<LEDA);
PORTB &= ~(1<<LEDV);
PORTB &= ~(1<<LEDR);
_delay_ms(300);
PORTD &= ~(1<<LEDP);//Apagamos Led en Baby Orangutan.
PORTB |= (1<<LEDA);
PORTB |= (1<<LEDV);
PORTB |= (1<<LEDR);
_delay_ms(300);
PORTD |= (1<<LEDP);
PORTB &= ~(1<<LEDA);
PORTB &= ~(1<<LEDV);
PORTB &= ~(1<<LEDR);
_delay_ms(300);
PORTD &= ~(1<<LEDP);
PORTB |= (1<<LEDA);
PORTB |= (1<<LEDV);
PORTB |= (1<<LEDR);
_delay_ms(300);
PORTD |= (1<<LEDP);
PORTB &= ~(1<<LEDA);
PORTB &= ~(1<<LEDV);
PORTB &= ~(1<<LEDR);
_delay_ms(300);
PORTD &= ~(1<<LEDP);
PORTB |= (1<<LEDA);
PORTB |= (1<<LEDV);
PORTB |= (1<<LEDR);
}
//Funciones para controlar la velocidad y dirección de los
//motores. PWM controla la velocidad, valor entre 0-255.
void M1_forward(unsigned char pwm)
{
OCR0A = 0;
OCR0B = pwm;
}
void M1_reverse(unsigned char pwm)
{
OCR0B = 0;
OCR0A = pwm;
}
void M2_reverse(unsigned char pwm)
{
OCR2A = pwm;
OCR2B = 0;
}
void M2_forward(unsigned char pwm)
{
OCR2B = pwm;
OCR2A = 0;
}
//Configuración del hardware del micro que controla los motores.
void motors_init(void)
{
// configure for inverted PWM output on motor control pins:
// set OCxx on compare match, clear on timer overflow
// Timer0 and Timer2 count up from 0 to 255
TCCR0A = TCCR2A = 0xF3;
// use the system clock/8 (=2.5 MHz) as the timer clock
TCCR0B = TCCR2B = 0x02;
// initialize all PWMs to 0% duty cycle (braking)
OCR0A = OCR0B = OCR2A = OCR2B = 0;
// set PWM pins as digital outputs (the PWM signals will not
// appear on the lines if they are digital inputs)
DDRD |= (1 << PORTD3) | (1 << PORTD5) | (1 << PORTD6);
DDRB |= (1 << PORTB3);
}
void inicializar_timer1(void) //Configura el timer y la interrupción.
{
OCR1A= 0x0138; //0C35 10 ms //0x0271 2ms
TCCR1B
|=((1<<WGM12)|(1<<CS11)|(1<<CS10));
//Los bits que no se tocan a 0 por defecto
TIMSK1 |= (1<<OCIE1A);
sei();
}
int obtener_errorp(void)
{
char errorp=0;
static char ultimo_errorp=0;
char contador_sensor=0;
if(((PINC & 0x01) != 0) && ((PIND & 0x01) != 0))
{
errorp=0;
return(0);
}
if((PINC & 0x08) != 0) //I3 PC3 -7
{
errorp = errorp - 0x07;
contador_sensor++;
}
if((PINC & 0x04) != 0) //I2 PC2 -5
{
errorp = errorp - 0x05;
contador_sensor++;
}
if((PINC & 0x02) != 0) //I1 PC1 -3
{
errorp = errorp - 0x03;
contador_sensor++;
}
if((PINC & 0x01) != 0) //I0 PC0 -1
{
errorp = errorp - 0x01;
contador_sensor++;
}
if((PIND & 0x01) != 0) //D0 PD0 +1
{
errorp = errorp + 0x01;
contador_sensor++;
}
if((PIND & 0x04) != 0) //D1 PD2 +3
{
errorp = errorp + 0x03;
contador_sensor++;
}
if((PIND & 0x10) != 0) //D2 PD4 +5
{
errorp = errorp + 0x05;
contador_sensor++;
}
if((PIND & 0x80) != 0) //D3 PD7 +7
{
errorp = errorp + 0x07;
contador_sensor++;
}
if(contador_sensor != 0)
{
errorp = errorp / contador_sensor;
ultimo_errorp = errorp;
return(Kp * (int)errorp);
}
else
{
if(ultimo_errorp < 0)
errorp = -0x09;
else
errorp = 0x09;
ultimo_errorp = errorp;
return((int)errorp * Kp);
}
}
int obtener_errord(void)
{
int error = 0;
static int error_old = 0;
static int errord=0;
static int errord_old = 0;
static int tic = 1;
static int tic_old = 1;
int diferencia = 0;
if(((PINC & 0x01) != 0) && ((PIND & 0x01) != 0))
error=0;
else if((PIND & 0x01) != 0) //D0 PD0 +1
error = 1;
else if((PINC & 0x01) != 0) //I0 PC0 -1
error = -1;
else if((PIND & 0x04) != 0) //D1 PD2 +3
error = 3;
else if((PINC & 0x02) != 0) //I1 PC1 -3
error = -3;
else if((PIND & 0x10) != 0) //D2 PD4 +5
error = 5;
else if((PINC & 0x04) != 0) //I2 PC2 -5
error = -5;
else if((PIND & 0x80) != 0) //D3 PD7 +7
error = 7;
else if((PINC & 0x08) != 0) //I3 PC3 -7
error = -7;
else
{
if (error_old < 0)
error = -9;
else if(error_old > 0)
error = 9;
}
//Cálculo de la velocidad media del error.
if (error == error_old)
{
tic = tic + 1;
if(tic > 30000)
tic = 30000;
if(tic > tic_old)
errord = errord_old/tic;
}
else
{
tic++;
diferencia = error - error_old;
errord = Kd*(diferencia)/tic; //error medio
errord_old = errord;
tic_old=tic;
tic=0;
}
error_old = error;
return(errord);
}
ISR(TIMER1_COMPA_vect)
{
PORTD |=(1<<LEDP);
int errort=0;
int proporcional = obtener_errorp();
int derivativo = obtener_errord();
errort = proporcional + derivativo;
if(errort > velocidad)
errort = velocidad;
else if(errort < - velocidad)
errort = - velocidad;
if(errort>0)
{
M2_forward(velocidad - errort); //Motor derecho.
M1_forward(velocidad);
//Motor izquierdo.
PORTB &= ~(1<<LEDV);
PORTB |= (1<<LEDR);
PORTB |= (1<<LEDA);
}
else if(errort<0)
{
M2_forward(velocidad);
//Motor derecho.
M1_forward(velocidad + errort); //Motor izquierdo.
PORTB &= ~(1<<LEDR);
PORTB |= (1<<LEDV);
PORTB |= (1<<LEDA);
}
else
{
M2_forward(velocidad);
M1_forward(velocidad);
PORTB |= (1<<LEDV);
PORTB |= (1<<LEDR);
PORTB &= ~(1<<LEDA);
}
PORTD &= ~(1<<LEDP);
TIFR1 |= (1<<OCF1A);
}