cmd_lm629.c

16F876 控制 LM629 的原程序.

#include "consts.h"
#include "i_LM629.h"
#include "cmd_LM629.h"
#include "iPIC.h"
#include "math.h"

//VERIF CHARGEMENTS TRAJECTOIRE : LES AUTRES BITS ??? (poids fort)

int32 dest_pos1;
int32 dest_pos2;


//Initialisation des param鑤res de trajectoire
void initLmData(int8 vitesse,int8 accel)
{
	//LM1
	execCmd1(LTRJ);
	//chargement vitesse + accel
	sendData1(LTRJ_BIT_ACCEL|LTRJ_BIT_VIT);
	sendData1(accel);sendData1(0x00);
	sendData1(vitesse);sendData1(0x00);
	
	//LM2
	execCmd2(LTRJ);
	//chargement vitesse + accel
	sendData2(LTRJ_BIT_ACCEL|LTRJ_BIT_VIT);
	sendData2(accel);sendData1(0x00);
	sendData2(vitesse);sendData1(0x00);
}

//Avance
void avance(int8 nb_pas,int8 sens)
{

	if(sens == AVANCE_AV)
	{
		dest_pos1 = nb_pas;
		dest_pos2 = nb_pas;
		
	}
	else
	{
		dest_pos1 = -nb_pas;
		dest_pos2 = -nb_pas;		
	}
		
	dest_pos1 *= FACTEUR_POS;
	dest_pos2 *= FACTEUR_POS;
			
	startMvt();
}

//Tourne
void tourne(int8 nb_pas,int8 sens)
{	
	if(sens == TOURNE_D)
	{
		dest_pos1 = nb_pas;
		dest_pos2 = - nb_pas;
	}
	else
	{
		dest_pos1 = - nb_pas;
		dest_pos2 = nb_pas;
	}
	
	dest_pos1 *= FACTEUR_POS;
	dest_pos2 *= FACTEUR_POS;
	
	startMvt();
}

void startMvt()
{
	//LM1
	execCmd1(LTRJ);
	sendData1(LTRJ_BIT_POS);
	sendData1((dest_pos1 >> 16) & 0x00FF);sendData1((dest_pos1) & 0x00FF);
	
	//LM2
	execCmd2(LTRJ);
	sendData2(LTRJ_BIT_POS);
	sendData2((dest_pos2 >> 16) & 0x00FF);sendData2((dest_pos2) & 0x00FF);
	
	//Start
	execCmd1(STT);
	execCmd2(STT);
}

void setVitesse(int8 val)
{
	float val_lm=0;
	float integral=0;
	int16 envoi_lm=0;
	
	execCmd1(LTRJ);
	//chargement vitesse + accel
	sendData1(LTRJ_BIT_VIT);
	val_lm =val*FACTEUR_VIT;
	integral=val_lm;
	val_lm=modf(envoi_lm,&integral); //val_lm=0.******* soit uniquement la partie d閏imale
	val_lm=val_lm*10000	;			// On a tte la partie d閏imale dans la partie enti鑢e de val_lm
	
	envoi_lm=abs(integral);  
	sendData1(envoi_lm);		//envoi de la partie enti鑢e
	
	envoi_lm=Abs(val_lm); 
	sendData1(envoi_lm);		//envoi la partie d閏imale
}

void setAccel(int8 val)
{
	float val_lm=0;
	float integral=0;
	int16 envoi_lm=0;
	
	execCmd1(LTRJ);
	//chargement vitesse + accel
	sendData1(LTRJ_BIT_ACCEL);
	val_lm =val*FACTEUR_ACC;
	integral=val_lm;
	val_lm=modf(envoi_lm,&integral); //val_lm=0.******* soit uniquement la partie d閏imale
	val_lm=val_lm*10000;			 // On a tte la partie d閏imale dans la partie enti鑢e de val_lm
	
	envoi_lm=abs(integral);  
	sendData1(envoi_lm);		//envoi de la partie enti鑢e
	
	envoi_lm=Abs(val_lm); 
	sendData1(envoi_lm);		//envoi la partie d閏imale
}

// Stop SANS MEMORISATION
void stop()
{
	//Arret Abrupt
	
	//LM1
	execCmd1(LTRJ);
	sendData1(LTRJ_HAND_BRAKE);
	
	//LM2
	execCmd2(LTRJ);
	sendData2(LTRJ_HAND_BRAKE);
}

// Stop AVEC MEMORISATION
int8 stop_temp()
{
	int8 retour;
	int16 dataLM;
	int8 data;
	
	//Arret Abrupt 
	stop();
	//MEMORISATION et Renvoi de la distance parcourue
	//EEPROM_MEM_STOP
	
	
	return data;
}

void resume()
{
	//On reprend les derniers param鑤res
	startMvt();
}

unsigned int8 waitForInt()
{
	unsigned int8 rep;	
	unsigned int1 pause;
	pause = false;
	
	do
	{
		do
		{
			//attendSignal(!ATT_BLOQ);
			attendSignal();
			rep = readOctet();
			
			switch(rep)
			{
				case CMD_STOP :
					stop();	
					return;
					break;
				case CMD_STOP_TEMP :
					pause = true;
					stop();
					break;
				case CMD_RESUME :
					pause = false;
					resume();
					break;
			}
		} while(pause);

		readStatus1();
		readStatus2();
		
	} while( !((g_data1 | g_data2) & STATUS_ERRORS)
			&& !(g_data1 & g_data2 & STATUS_TRJ_END) );

	if(g_data1 & g_data2 & STATUS_TRJ_END)
	{
		//Trajectoire termin閑
		rep = TRJ_OK;
	}
	else
	{
		//Erreur survenue
		if((g_data1 | g_data2) & STATUS_DEP_ESP)
		{
			//D閜assement d'espace pour la position
			rep = ERR_DEP;
		}
		
		if((g_data1 | g_data2) & STATUS_POS_ERR)
		{
			//Erreur de position
			rep = ERR_POS;
		}
	}
	
	return rep;
}