98 lines
2.3 KiB
Plaintext
98 lines
2.3 KiB
Plaintext
#include "global.th";
|
|
#include "sensors.th";
|
|
|
|
#define ENCWORKER 5
|
|
EncoderProcess works[ENCWORKER];
|
|
string vector[4];
|
|
char portVector[SENSORS];
|
|
int calibrate_mode = 0;
|
|
|
|
|
|
void encoder_init(){
|
|
for (int i = 0; i< SENSORS;i++){
|
|
portVector[i]=-1;
|
|
}
|
|
int intnum = ((int)(config.EncoderCfg.encoderPort/4));
|
|
for (char i = 0; i < ENCWORKER; i++){
|
|
works[i].Enabled = false;
|
|
works[i].config = &config.EncoderCfg;
|
|
works[i].State = false;
|
|
works[i].Count = 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);
|
|
}
|
|
}
|
|
#if PLATFORM_ID != SIMULATOR
|
|
io.intnum = intnum-1;
|
|
io.intenabled = YES;
|
|
#endif
|
|
|
|
}
|
|
|
|
bool getEncEnabled(unsigned char port,bool value){
|
|
if (port == config.EncoderCfg.encoderPort)
|
|
return false;
|
|
if (portVector[port]==-1)
|
|
return true;
|
|
|
|
if (works[portVector[port]].Enabled && works[portVector[port]].State == false && value){
|
|
long interval = works[portVector[port]].config->encoderInterval;
|
|
long diff = interval /5;
|
|
|
|
if (works[portVector[port]].Count >= (interval/2 - diff) && works[portVector[port]].Count <= (interval/2+diff)){
|
|
works[portVector[port]].State = true;
|
|
}
|
|
return works[portVector[port]].State;
|
|
}
|
|
else{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
void on_io_int(unsigned char linestate){
|
|
string workers = vector[linestate-1];
|
|
for (int i = 0; i < len(workers);i++){
|
|
if (works[i].Enabled){
|
|
works[i].Count++;
|
|
if (works[i].Count>works[i].config->encoderInterval)
|
|
{
|
|
works[i].Count = 0;
|
|
works[i].State = false;
|
|
}
|
|
d("count ["+str(i)+"] :"+str(works[i].Count));
|
|
}
|
|
}
|
|
|
|
}
|
|
/// Блок калибровки
|
|
|
|
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;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
int enc_calibrate_result(){
|
|
|
|
return 0;
|
|
} |