tibbo_line/encoder.tc

#include "global.th";
#include "sensors.th";
#include "command.th"

#define ENCWORKER 5
EncoderProcess works[ENCWORKER];
string vector[4];
char portVector[SENSORS];
int calibrate_mode = 0;
unsigned long calibrate_buff[ENCWORKER][10];
unsigned long calibrate_count[ENCWORKER];
char z[ENCWORKER]; 
bool inited = false;
int center;
unsigned long count;

#if PLATFORM_ID==TPP2WG2
	int interrupt[4] = {1,2,1,2};
#endif
#if PLATFORM_ID==TPP3WG2
	int interrupt[4] = {1,5,2,6};
#endif


void encoder_init(){
	count = 0;
	config.STORE_ENCODER = false;
	for (int i = 0; i< SENSORS;i++){
		portVector[i]=-1;
	}
	int intnum = interrupt[((int)(config.EncoderCfg.encoderPort/4))-1];
	center = config.EncoderCfg.encoderInterval/2;
	for (char i = 0; i < ENCWORKER; i++){
		works[i].Enabled = false;
		works[i].config = &config.EncoderCfg;
		works[i].State = false;
		works[i].Inited = false;
		works[i].Count = config.EncoderCfg.encoderInterval/2;
		works[i].Point = config.EncoderCfg.encoderInterval/2;
		works[i].WorkPort=0;
		string ports = config.EncoderCfg.workedPorts;
		if (i<len(ports)){
			works[i].WorkPort = config.EncoderCfg.workedPorts[i];
			portVector[works[i].WorkPort] = i;
			works[i].Enabled = true;
			vector[intnum-1]+=chr(i);
			config.STORE_ENCODER = true;
		}
	}
	inited = true;
#if PLATFORM_ID != SIMULATOR
	io.intnum = intnum-1;
	//if (config.STORE_ENCODER) 
	io.intenabled = YES;
#endif
}

bool inEncCalibrate(){
	return calibrate_mode>0;
}

bool getEncEnabled(unsigned char port,bool value){
	if (!inited) return true;
	if (calibrate_mode>0) // в режиме калибровки не слать состояние портов
		return false;
	if (port == config.EncoderCfg.encoderPort)
		return false;
	if (portVector[port]==-1)
		return true;
	t("enc_start");
	if (works[portVector[port]].Enabled && works[portVector[port]].State == false && value){
		long interval = works[portVector[port]].config->encoderInterval;
		long diff = interval * config.EncoderCfg.windowWidth / 200;
		if (works[portVector[port]].Inited == false){
			works[portVector[port]].Count = interval/2;
			works[portVector[port]].Inited =true;
			if (config.debug.EncoderDebug>0) d(str(port)+" выравнивание по первой ");
			z[portVector[port]]=0;
		}
		int point = (works[portVector[port]].Point + works[portVector[port]].Count) /2;
		int sdvig = center - point;

		if (config.debug.EncoderDebug>3) d(str(port)+ " count:"+str(works[portVector[port]].Count)+" center:"+str(center)+" point:"+str(point)+" diff:"+str(diff)+" sdvig:"+stri(sdvig));
		
		if (works[portVector[port]].Count >= (interval/2 - diff) && works[portVector[port]].Count <= (interval/2+diff)){
			works[portVector[port]].State = true;
			if (config.debug.EncoderDebug>2) d(str(port)+" попал о окно "+str(works[portVector[port]].Count));
			works[portVector[port]].Point = (works[portVector[port]].Point + works[portVector[port]].Count) /2;
			//int point = works[portVector[port]].Point;
			//int sdvig = center - point;
			int d1=diff/2;
			if (sdvig > d1 || sdvig < 0-d1 ){
				int nc = center;
				if (config.debug.EncoderDebug>1) d(str(port)+" "+ str(works[portVector[port]].Count)+" "+stri(sdvig)+" "+(sdvig < 0 ? " << " : " >> ")+" new "+str(nc));
				works[portVector[port]].Count = nc;
				
			}
		}else{
			if (config.debug.EncoderDebug>2) d(str(port)+" не попал о окно "+str(works[portVector[port]].Count));
		}
		if (works[portVector[port]].State) {
			send_encoder_tick(port,count);
		}
		t("enc_end");
		return works[portVector[port]].State;
	}
	else{
		if (works[portVector[port]].Enabled && works[portVector[port]].State == true && value){
			if (config.debug.EncoderDebug>1) d(str(port)+" Повторное срабатывание "+str(works[portVector[port]].Count));
		} 
		t("enc_end");
		return false;
	}
}



void on_io_int(unsigned char linestate){
	t("enc_int_start");
	count ++;
	//string workers = vector[linestate-1];
	if(calibrate_mode>0){ // калибровка
		for (int i=0;i<len(config.EncoderCfg.workedPorts);i++){
			unsigned char port = config.EncoderCfg.workedPorts[i];
			if (pinChange(port) && getPinStateS(port,true)==true)
			{
				if (z[i]>=0)
				{
					calibrate_buff[i][z[i]]=calibrate_count[i];
					cmd_replay("Калибровка энкодера "+str(port)+":"+str(calibrate_count[i]));
					if (config.debug.EncoderDebug>0) d("Калибровка энкодера "+str(port)+":"+str(calibrate_count[i]));
					send_encoder_tick(port,count);
				}
				z[i]++;
				calibrate_count[i]=0;
				if (z[i]>9) 
					z[i]=0;
			}
			else
				calibrate_count[i]++;
		}
	}
	else{
		for (int i = 0; i < ENCWORKER;i++){
			if (works[i].Enabled){
				works[i].Count++;
				if (works[i].Count>works[i].config->encoderInterval)
				{
					z[i] = (works[i].State? 0: z[i]+1);
					//d("z["+str(i)+"] = "+ str(z[i])+" | "+str(config.EncoderCfg.reinitInterval));
					if (z[i] >= config.EncoderCfg.reinitInterval)
						works[i].Inited = false;
					works[i].Count = 0;
					works[i].State = false;
				}
				//d("count ["+str(i)+"] :"+str(works[i].Count));
			}
		}
	}
	t("enc_int_end");
}
/// Блок калибровки 

void enc_calibrate_mode(int mode){
	switch (mode){
		case 0: 	// Остановка калибровки
			calibrate_mode = 0;
			break;
		case 1:		// Калибровка интервала бутылок
			calibrate_mode = 1;
			break;
		case 2:		// Калибровка интервала датчиков
			calibrate_mode = 2;
			break;
		default:
			calibrate_mode = 0;
			d("Установка режима калибровки энкодера - неверный режим: "+ str(mode));
		break;
	}
	if (calibrate_mode>0){
		for (int i=0;i<len(config.EncoderCfg.workedPorts);i++){
			z[i]=-1;
			calibrate_count[i]=0;
			for (int x=0;x<10;x++){
				calibrate_buff[i][x]=0;
			}
		}
	}
}


int enc_calibrate_result(){
	int o = 0;
	for (int i=0;i<len(config.EncoderCfg.workedPorts);i++){
			int c = 0;
			int s = 0;
			for (int x=0;x<10;x++){
				if (calibrate_buff[i][x]>0){
					c++;
					s+=calibrate_buff[i][x];
				}
			}
			o += s/c;
			if (o > s/c)
				o = o/2;
		}
	return o;
}