/*

* BRAZO_ROBOTIOCO_2_GRADOS.c
*
* Created: 12/10/2018 11:06:31
* Author : JeanPaul
*/
#define F_CPU 16000000
#include <avr/io.h>
#include <stdbool.h>
#include <math.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#define PI 3.14159265

float parametros[]={1,250,250,0,500,1};

bool ready=false;
//*************************LONGITUD DE LOS BRAZO*****************
float L1=250;
float L2=250;
//*************************ANGULOS*******************************
float alpha1;
float alpha2;

void iniciar_usart();
void setup_TIMER1();
void recta();

int main(void)
{
DDRC=0XFF;
DDRA=0XFF;
iniciar_usart();
setup_TIMER1();
while (1)
{
float opcion=parametros[0];
while (opcion==1)
{
recta();
}
while (opcion==2)
{

}
while (opcion==3)
{
}
}
}

void recta()
{
ready=parametros[5];
if(ready==true)
 {
float AX=parametros[1];
float AY=parametros[2];
float BX=parametros[3];
float BY=parametros[4];
//****************************COORDENADAS A DEZPLAZARSE(C)*********
float x;
float y;

float pendiente;
int16_t punto=0;
if (AY==BY)
{
pendiente=0;
}
if(AY<BY)
{
pendiente=(BY-AY)/(BX-AX);
}
else
{
pendiente=(AY-BY)/(AX-BX);
}

//*************PRIMER CASO PENDIENTE CRECIENTE MENOR A 1 Y MAYOR 0

************
if (pendiente<=1)
{
PORTC=0XFF;
x=AX;
punto=BX-AX;
if (punto<0)
{
punto=(-1)*punto;
}

for(uint16_t i=0;i<=punto;i++)
{

y=pendiente*(x-AX)+AY;

alpha2=acos((pow(x,2)+pow(y,2)-pow(L1,2)-pow(L2,2))/
(2*L1*L2))* 180.0 / PI;

alpha1=atan(y/x)*180 / PI -atan((L2*sin(alpha2*PI/180))/
(L1+L2*cos(alpha2*PI/180)))*180/ PI;

if (x<0)
{
alpha1=180+alpha1;
}

OCR1A=21*alpha1+1000;
OCR1B=1000+alpha2*21;
_delay_ms(10);
if (AX<BX)
{
x++;
 }
else
{

x--;
}
PORTC=0X00;
}

//*************SEGUNDO CASO PENDIENTE CRECIENTE MAYOR A 1 Y MAYOR 0

************
if (pendiente>1 || pendiente<-1)
{
PORTA=0XFF;
y=AY;
punto=BY-AY;

if (punto<0)
{
punto=-1*punto;
}

for(uint16_t i=0;i<=punto;i++)
{

x=(y-AY)/pendiente+AX;

alpha2=acos((pow(x,2)+pow(y,2)-pow(L1,2)-pow(L2,2))/
(2*L1*L2))* 180/ PI;

alpha1=atan(y/x)*180 / PI -atan((L2*sin(alpha2*PI/180))/
(L1+L2*cos(alpha2*PI/180)))*180/ PI;

if (x<0)
{
alpha1=180+alpha1;
}

OCR1A=21*alpha1+1000;
OCR1B=1000+alpha2*21;
_delay_ms(10);

if (AY<BY)
{
y++;
}
else
{
y--;
}
}
PORTA=0x00;
}

_delay_ms(2000);
}
 ready=false;
parametros[5]=0;

void setup_TIMER1()
{
TCCR1A=(1<<COM1A1)|(0<<COM1A0)|(1<<COM1B1)|(0<<COM1B1)|(1<<WGM11)|(0<<WGM10);
TCCR1B=(1<<WGM13)|(1<<WGM12)|(0<<CS12)|(1<<CS11)|(0<<CS10);
TCNT1=1000;
OCR1A=1000;
OCR1B=1000;
ICR1=40000;
TIMSK=0;
TIFR=0;
DDRD|=(1<<PD5);
DDRD|=(1<<PD4);
}
ISR(USART_RXC_vect)
{
for (int i=0;i<6;i++)
{

parametros[i]=UDR;
_delay_ms(200);

void iniciar_usart(){
UCSRB=0b10011000;
UCSRC=0b00000110;
UBRRH=0;
UBRRL=103;
}