/////////////////////////////////
// LAB II
// Robot.axe: Projecte de configuració.
// v1.0 - (c)2001 DEE-UPC
// Robot.c:Programa principal del projecte  
/////////////////////////////////

#include <embedded.h>
#include "typedefs.h"
#include "register.h"
#include "timers.h"	
#include "ports.h"
#include "int.h"
#include "register2.h"

//definimos los movimientos del motor para puerto 2	
#define motor_antihorario 	0xFE
#define motor_horario 		0xFD
#define motor_atras 		0xFB
#define motor_adelante 		0xF7
#define motor_arriba 		0xEF
#define motor_abajo 		0xDF
#define motor_abrir_pinza 	0xBF
#define motor_cerrar_pinza 	0x7F
#define motor_parados 		0xFF

//definimos movimentos mando
#define abrir_pinza 0x70
#define cerrar_pinza 0x74
#define subir 0x72
#define bajar 0x76
#define almacenar 0x7A
#define atras 0x71
#define adelante 0x75
#define borrar 0x79
#define giro_horario 0x77
#define giro_antihorario 0x73
#define reproducir 0x7E
#define inicio 0x7D

#define MAX 10
#define NUM_PASOS_LENTOS 5

typedef struct {
	int CV1;
	int CV2;
	int CV3;
	int CV4;
}Tposicion;

typedef struct {
	Tposicion posicion[MAX];
   int indice;
}Tposicion_guardada;

//Variables Globales
BYTE rebut=0;
BYTE datos_tecla=0;
BYTE dades_canal=0x00;
BYTE movimiento=0x00;
Tposicion posicion;
BYTE contador_pasos;
BYTE timer;
BYTE port_PT=0;
Tposicion_guardada posicion_guardada; //vector donde almacenamos las posiciones
BYTE modo_lento;
BYTE mover_lento;

void agafar_dades_canal_serie(void)
{

	dades_canal = *(BYTE far *)MK_FP(RSEG, RxB0);
   
}  // Llegeix canal serie 0

void inici(){

 	//esperem a que tots els motors arribin a final de carrera

	while((GetPT()&0x01 )== 0x01){
		SetP2(0xAA);
      Retard(20);
	}
	SetP2(0xFF);
   posicion.CV1=posicion.CV2=posicion.CV3=posicion.CV4=0;

}
void reset (void){
	configuracio_canal_serie(); //configuracion del canal serie 0
   configuracio_int_canal_serie();
 	IniPorts();	//config. ports
   IniTimers();	//config. temporitzadors
	inici();	//rutina d'inici
   movimiento=0x00;
   rebut=0;
}



void configuracio_canal_serie (void){
	BYTE far *registre;
   registre = (BYTE far *)MK_FP(RSEG, SCM0);
   *registre = 0xC1; //configurem SCM0 per: cap bit de paritat
   									//un bit de stop, 7 bit de dades,
                              //i recepcio permesa
   registre = (BYTE far *)MK_FP(RSEG, BRG0);    //donem el valor de G=122
   *registre = 122;

  	registre = (BYTE far *)MK_FP(RSEG, SCC0);    //donem el valor de n=6
   *registre = 5;
}


void configuracio_int_canal_serie(void)
{

   BYTE far *bitmascara;
   disable();
   bitmascara = (BYTE far *)MK_FP(RSEG, SRIC0);
   *bitmascara = *bitmascara & 0xBF;
   enable();

}

void interrupt RSIcanal_serie0(void){
	agafar_dades_canal_serie();
	enable();
	rebut=1;
   	FINT;
}
void enviar_movimiento(BYTE movimiento){
  	if(modo_lento==0)
   {
			SetP2(movimiento);
   		Retard(20);
      	SetP2(0xFF);
   }
   else
   {		if(mover_lento==0)

   		{  SetP2(movimiento);
         	Retard(20);
      		SetP2(0xFF);
         }
   }

}

void interrupt RSI_Timer0(){
	enable();
	timer++;
	contador_pasos=GetPT();
	if(port_PT!=contador_pasos){
		if((port_PT&0x02)!=(contador_pasos&0x02)){
			if(movimiento==motor_antihorario)
				posicion.CV1--;
			if(movimiento==motor_horario)
				posicion.CV1++;
		}
		if((port_PT&0x04)!=(contador_pasos&0x04)){
			if(movimiento==motor_atras)
				posicion.CV2--;
			if(movimiento==motor_adelante)
				posicion.CV2++;
		}
		if((port_PT&0x08)!=(contador_pasos&0x08)){
			if(movimiento==motor_arriba)
				posicion.CV3--;
			if(movimiento==motor_abajo)
				posicion.CV3++;
		}
		if((port_PT&0x10)!=(contador_pasos&0x10)){
			if(movimiento==motor_abrir_pinza)
				posicion.CV4--;
			if(movimiento==motor_cerrar_pinza)
				posicion.CV4++;
		}
		timer=0;
	}
	port_PT=contador_pasos;
	FINT;
}
void interrupt RSI_timer1(void){
	enable();
   if(modo_lento==1){
   	mover_lento++;
      if(mover_lento>=4)
      mover_lento=0;
   }
   FINT;
}
void guardar(void){
	int indice;
   indice=posicion_guardada.indice;
   if (indice==0){
   	posicion_guardada.posicion[0]=posicion;
      posicion_guardada.indice=1;
      }
   if(indice>0 && indice<MAX){
   	if(!((posicion_guardada.posicion[indice-1].CV1==posicion.CV1)&& (posicion_guardada.posicion[indice-1].CV2==posicion.CV2)&&(posicion_guardada.posicion[indice-1].CV3==posicion.CV3)&&(posicion_guardada.posicion[indice-1].CV4==posicion.CV4))){
      	posicion_guardada.posicion[indice]=posicion;
         posicion_guardada.indice++;
         }
      }
}


void calcula_diferencia(int paso_actual,int paso_futuro,int num_motor){
	int dif;
   dif=(paso_futuro-paso_actual);
   if(dif>0){
   	switch(num_motor){
      	case 1:
         	while(posicion.CV1<paso_futuro-5){
            	SetP2(motor_horario);
               movimiento=motor_horario;
               Retard(20);
               }
            while((posicion.CV1>=paso_futuro-5)&&(posicion.CV1<paso_futuro))
            	modo_lento=1;
            modo_lento=0;
            break;
           case 2:
         	while(posicion.CV2<paso_futuro-5){
            	SetP2(motor_adelante);
               movimiento=motor_adelante;
               Retard(20);
               }
            while((posicion.CV2>=paso_futuro-5)&&(posicion.CV2<paso_futuro))
            	modo_lento=1;
            modo_lento=0;
            break;
           case 3:
         	while(posicion.CV3<paso_futuro-5){
            	SetP2(motor_abajo);
               movimiento=motor_abajo;
               Retard(20);
               }
            while((posicion.CV3>=paso_futuro-5)&&(posicion.CV3<paso_futuro))
            	modo_lento=1;
            modo_lento=0;
            break;
           case 4:
         	while(posicion.CV4<paso_futuro-5){
            	SetP2(motor_cerrar_pinza);
               movimiento=motor_cerrar_pinza;
               Retard(20);
               }
            while((posicion.CV4>=paso_futuro-5)&&(posicion.CV4<paso_futuro))
            	modo_lento=1;
            modo_lento=0;
            break;
         }
      }
    else if(dif<0){
         switch(num_motor){
      	case 1:
         	while(posicion.CV1>paso_futuro+5){
            	SetP2(motor_antihorario);
               movimiento=motor_antihorario;
               Retard(20);
               }
            while((posicion.CV1<=paso_futuro-5)&&(posicion.CV1>paso_futuro))
            	modo_lento=1;
            modo_lento=0;
            break;
           case 2:
         	while(posicion.CV2>paso_futuro+5){
            	SetP2(motor_atras);
               movimiento=motor_atras;
               Retard(20);
               }
            while((posicion.CV2<=paso_futuro-5)&&(posicion.CV2>paso_futuro))
            	modo_lento=1;
            modo_lento=0;
            break;
            case 3:
         	while(posicion.CV3>paso_futuro+5){
            	SetP2(motor_arriba);
               movimiento=motor_arriba;
               Retard(20);
               }
            while((posicion.CV3<=paso_futuro-5)&&(posicion.CV3>paso_futuro))
            	modo_lento=1;
            modo_lento=0;
            break;
            case 4:
         	while(posicion.CV4>paso_futuro+5){
            	SetP2(motor_abrir_pinza);
               movimiento=motor_abrir_pinza;
               Retard(20);
               }
            while((posicion.CV4<=paso_futuro-5)&&(posicion.CV4>paso_futuro))
            	modo_lento=1;
            modo_lento=0;
            break;
         }
      }
    }
void reproducir_posiciones(void){
	BYTE i;
   int pas_actual;
   inici();
   modo_lento=0;
   for(i=0;i<posicion_guardada.indice;i++){
       pas_actual=posicion.CV1;
       calcula_diferencia(pas_actual,posicion_guardada.posicion[i].CV1,1);
       pas_actual=posicion.CV2;
       calcula_diferencia(pas_actual,posicion_guardada.posicion[i].CV2,2);
       pas_actual=posicion.CV3;
       calcula_diferencia(pas_actual,posicion_guardada.posicion[i].CV3,3);
       pas_actual=posicion.CV4;
       calcula_diferencia(pas_actual,posicion_guardada.posicion[i].CV4,4);
   	}
   }
void main()
{
	disable();
	setvect(INTSRX0,RSIcanal_serie0);  //ficar la nostra RSI del canal serie 0
   setvect(INTCOUNTER0,RSI_Timer0);   //ficar la nostra RSI del timer0
   setvect(INTCOUNTER1,RSI_timer1);
   enable();



   //fem un llaç infinit de control
   for(;;)	{
   	      if (rebut){
            	 SetP2(dades_canal);
               	 rebut=0;
               	 switch(dades_canal){
		   					case abrir_pinza:
								movimiento=motor_abrir_pinza;
							  	enviar_movimiento(movimiento);
                  		break;
                   		case cerrar_pinza:
                  		movimiento=motor_cerrar_pinza;
                     	enviar_movimiento(movimiento);
                  		break;
                   		case subir:
                  		movimiento=motor_arriba;
                     	enviar_movimiento(movimiento);
                  		break;
                   		case bajar:
                  		movimiento=motor_abajo;
                     	enviar_movimiento(movimiento);
                  		break;
                   		case almacenar:
                  	  	break;
                   		case atras:
                  		movimiento=motor_atras;
                     	enviar_movimiento(movimiento);
                  		break;
                   		case adelante:
                  		movimiento=motor_adelante;
                     	enviar_movimiento(movimiento);
                  		break;
                   		case borrar:
                  		break;
                   		case giro_antihorario:
                  		movimiento=motor_antihorario;
                     	enviar_movimiento(movimiento);
                  		break;
                   		case giro_horario:
                  		movimiento=motor_horario;
                     	enviar_movimiento(movimiento);
                  		break;
                   		case reproducir:
                  		break;
                   		case inicio:
                  		break;


			}
	}

     //Retard(APROX10S);
   }	//fi del llaç for



}