#define F_CPU 20000000UL
#include <avr/io.h>
#include <avr/eeprom.h>
#include <stdint.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <stdio.h>
#include <string.h>
#include <pololu/orangutan.h>
#include "velocistaB_2.h"
volatile int8_t boton;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/* Interrupción que salta cuando se pulsa el botón */
ISR(INT0_vect)
{
if(boton == 0) // se pulsa para calibrar
{
boton = 1;
}
else if(boton == 2) // se pulsa para moverse
{
boton = 3;
}
else if(boton == 5) // se pulsa para pararse
{
boton = 6;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/* Devuelve el valor máximo */
int max(int valor1, int valor2)
{
if(valor1 > valor2)
{
return valor1;
}
else
{
return valor2;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/* Devuelve el valor mínimo */
int min(int valor1, int valor2)
{
if(valor1 < valor2)
{
return valor1;
}
else
{
return valor2;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/* Enciende y apaga los leds repetitivamente */
void alarma()
{
uint16_t indice;
for(indice = 0; indice < 10; indice++)
{
PORTB &= ~((1 << PB1) | (1 << PB2));
delay_ms(200);
PORTB |= (1 << PB1) | (1 << PB2);
delay_ms(200);
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/* lee la carga de la batería a través del divisor de tensión */
int mili_voltios()
{
uint16_t indice;
long suma = 0;
static uint16_t medidas[] = {3700,3700,3700,3700,3700,3700,3700,3700,3700,3700};
if(analog_is_converting() != 0)
{
return -1;
}
for(indice = 0; indice < 9; indice++)
{
medidas[indice + 1] = medidas[indice];
}
medidas[0] = to_millivolts(analog_conversion_result());
for(indice = 0; indice < 10; indice++)
{
suma += medidas[indice];
}
start_analog_conversion(6);
return suma / 10;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/* Inicializamos todos los parámetros */
void inicializa()
{
uint16_t *maximos;
uint16_t *minimos;
unsigned char qtr_rc_pins[] = {IO_D4, IO_B0, IO_C5, IO_C4, IO_C3, IO_C2, IO_C1, IO_C0};
DDRB |= (1 << PB1) | (1 << PB2) | (1 << PD7);
MCUCR |= (1<<ISC01) | (1<<ISC00);
EIMSK |= (1 << INT0);
sei();
set_analog_mode(MODE_8_BIT);
start_analog_conversion(6);
serial_set_baud_rate(9600);
// Comprueba si hay valores guardados en la EEPROM para hacer o no la calibración
qtr_rc_init(qtr_rc_pins, 8, 5000, 255);
if(eeprom_read_byte(0) != 0)
{
qtr_calibrate(QTR_EMITTERS_ON);
maximos = qtr_calibrated_maximum_on();
minimos = qtr_calibrated_minimum_on();
eeprom_read_block (maximos, (const void *)0, 16);
eeprom_read_block (minimos, (const void *)16, 16);
PORTB |= (1 << PB1) | (1 << PB2);
boton = 2;
}
else
{
PORTB &= ~((1 << PB1) | (1 << PB2));
boton = 0;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/* Proceso de calibración de los sensores y guardado de los valores en la EEPROM */
void calibra()
{
uint16_t indice;
int16_t mvoltios;
uint16_t *maximos;
uint16_t *minimos;
for (indice = 0; indice < 10; indice++)
{
mvoltios = mili_voltios();
}
if(mvoltios != -1 && mvoltios < 3100) // Batería con menos de 3,1 v (en realidad menos de 6.2 v.)
{
alarma();
boton = 0;
return;
}
set_m1_speed(-PWM_CALIBRA);
set_m2_speed(PWM_CALIBRA);
for (indice = 0; indice < 250; indice++)
{
qtr_calibrate(QTR_EMITTERS_ON);
delay(20);
}
set_m1_speed(0);
set_m2_speed(0);
maximos = qtr_calibrated_maximum_on();
minimos = qtr_calibrated_minimum_on();
eeprom_write_block (maximos, (void *)0, 16);
eeprom_write_block (minimos, (void *)16, 16);
boton = 2;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/* El robot empieza a moverse y calculamos el PD */
void movimiento()
{
unsigned int sensores[8];
int posicion;
float cantidad_error;
static float anterior_error = 0;
int pid;
int mvoltios;
float mvoltios2;
int pwm1;
int pwm2;
mvoltios = mili_voltios();
if(mvoltios != -1 && mvoltios < 3100) // Batería con menos de 3,1 v (en realidad menos de 6,2 v.)
{
parada();
alarma();
return;
}
if(get_ms() >= 1000) // pasado 1 segundo ya no hay rebotes y se permite pulsar el botón de nuevo para parar el robot
{
boton = 5;
}
posicion = qtr_read_line(sensores, QTR_EMITTERS_ON);
cantidad_error = 3500 - posicion;
pid = cantidad_error * KP;
pid += (cantidad_error - anterior_error) * KD;
anterior_error = cantidad_error;
pwm1 = PWM_PUNTA;
pwm2 = PWM_PUNTA;
// hacia la izquierda
if(pid > 0)
{
pwm1 -= pid;
PORTB &= ~(1 << PB2);
PORTB |= (1 << PB1);
}
//hacia la derecha
else if(pid < 0)
{
pwm2 += pid;
PORTB &= ~(1 << PB1);
PORTB |= (1 << PB2);
}
// centrado
else
{
PORTB &= ~((1 << PB1) | (1 << PB2));
}
// Ajustamos el PWM según la carga de la batería
mvoltios2 = mvoltios / 1000;
pwm1 = (float)pwm1 * 3.7 / mvoltios2;
pwm2 = (float)pwm2 * 3.7 / mvoltios2;
pwm1 = max(min(pwm1, 255), 0);
pwm2 = max(min(pwm2, 255), 0);
set_m1_speed(pwm1);
set_m2_speed(pwm2);
delay_ms(2);
PORTD ^= (1 << PD7); //para comprobar la frecuencia
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/* Se para el motor */
void parada()
{
set_m1_speed(0);
set_m2_speed(0);
PORTB |= (1 << PB1) | (1 << PB2);
delay_ms(1000);
boton = 2;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int main(void)
{
inicializa();
while(1)
{
if(boton == 1)
{
calibra();
}
else if(boton == 3)
{
boton = 4;
time_reset();
}
else if(boton == 4 || boton == 5)
{
movimiento();
}
else if (boton == 6)
{
parada();
}
}
return 0;
}
