387 lines
11 KiB
C++
387 lines
11 KiB
C++
#include "IO.h"
|
|
#include "VarClass.h"
|
|
#include "motor.h"
|
|
#include "HmiClass.h"
|
|
#include "UtilClass.h"
|
|
|
|
void IoClass::Setup()
|
|
{
|
|
hmi.SendMessage("##:IO:SETUP", false);
|
|
|
|
byte DIPins[] = { 30,31,32,33,34,35,36,37,38,39,40,41,42,43,44,45 };
|
|
byte DOPins[] = { 22,23,24,25,26,27,28,29 };
|
|
|
|
//입력포트 설정
|
|
for (int i = 0; i < sizeof(DIPins); i++)
|
|
pinMode(DIPins[i], INPUT);
|
|
|
|
//출력포트설정
|
|
for (int i = 0; i < sizeof(DOPins); i++)
|
|
pinMode(DOPins[i], OUTPUT);
|
|
|
|
//MAF 초기화
|
|
for (int i = 0; i < MAF_SIZE; i++)
|
|
{
|
|
bufferF[i] = 0;
|
|
bufferB[i] = 0;
|
|
}
|
|
|
|
}
|
|
void IoClass::Update()
|
|
{
|
|
|
|
//Read DigitalInput
|
|
|
|
bool diValue[16]; int idx = 0;
|
|
diValue[idx++] = digitalRead(PINI_EMG); //B접이다
|
|
diValue[idx++] = digitalRead(PINI_BTN_1);
|
|
diValue[idx++] = digitalRead(PINI_BTN_2);
|
|
diValue[idx++] = digitalRead(PINI_BTN_3);
|
|
diValue[idx++] = digitalRead(PINI_BTN_4);
|
|
diValue[idx++] = digitalRead(PINI_OVERLOADL1);
|
|
diValue[idx++] = digitalRead(PINI_OVERLOADL2);
|
|
diValue[idx++] = digitalRead(PINI_OVERLOADR1);
|
|
diValue[idx++] = digitalRead(PINI_OVERLOADR2);
|
|
diValue[idx++] = digitalRead(PINI_EMPTY_38);
|
|
diValue[idx++] = digitalRead(PINI_BTN_ZUP);
|
|
diValue[idx++] = digitalRead(PINI_BTN_ZDN);
|
|
diValue[idx++] = digitalRead(PINI_LIMIT_LU);
|
|
diValue[idx++] = digitalRead(PINI_LIMIT_LD);
|
|
diValue[idx++] = digitalRead(PINI_LIMIT_RU);
|
|
diValue[idx++] = digitalRead(PINI_LIMIT_RD);
|
|
diValue[idx++] = digitalRead(PINI_STOP);
|
|
|
|
|
|
//비상정지 반전
|
|
diValue[IDXI_EMG] = !diValue[IDXI_EMG];
|
|
|
|
//오버로드센서 반전
|
|
diValue[IDXI_OVERLOADL] = !diValue[IDXI_OVERLOADL];
|
|
diValue[IDXI_OVERLOADR] = !diValue[IDXI_OVERLOADR];
|
|
diValue[IDXI_EMPTY7] = !diValue[IDXI_EMPTY7];
|
|
diValue[IDXI_EMPTY8] = !diValue[IDXI_EMPTY8];
|
|
|
|
//오버로드센서를 합친다 5+6=>5, 7+8=>6
|
|
diValue[IDXI_OVERLOADL] = diValue[IDXI_OVERLOADL] || diValue[IDXI_OVERLOADR];
|
|
diValue[IDXI_OVERLOADR] = diValue[IDXI_EMPTY7] || diValue[IDXI_EMPTY8];
|
|
diValue[IDXI_EMPTY7]= false;
|
|
diValue[IDXI_EMPTY8]= false;
|
|
|
|
//리밋센서도 반전
|
|
diValue[IDXI_LIMIT_LU] = !diValue[IDXI_LIMIT_LU];
|
|
diValue[IDXI_LIMIT_LD] = !diValue[IDXI_LIMIT_LD];
|
|
diValue[IDXI_LIMIT_RU] = !diValue[IDXI_LIMIT_RU];
|
|
diValue[IDXI_LIMIT_RD] = !diValue[IDXI_LIMIT_RD];
|
|
|
|
//리버스 체크 200325
|
|
//입력값은 Reverse 하지 않는다
|
|
/*
|
|
var._eep_pindir_iL = 0;
|
|
var._eep_pindir_iH = 0;
|
|
|
|
for (int i = 0; i < 16; i++)
|
|
{
|
|
if (i < 8)
|
|
{
|
|
if (bitRead(var._eep_pindir_iL, i) == true)
|
|
diValue[i] = !diValue[i];
|
|
}
|
|
else {
|
|
if (bitRead(var._eep_pindir_iH, i - 8) == true)
|
|
diValue[i] = !diValue[i];
|
|
}
|
|
}*/
|
|
|
|
|
|
for (int i = 0; i < idx; i++)
|
|
{
|
|
//이 값의 변화를 체크한다.
|
|
bool oldValue = bitRead(var.IOData, i);
|
|
bool isNewValue = false;
|
|
if (oldValue != diValue[i]) isNewValue = true;
|
|
bitWrite(var.IOData, i, diValue[i]);//0번부터 기록한다
|
|
|
|
//값이 변화했따
|
|
if (isNewValue) IOValueChanged(false, i, diValue[i]);
|
|
}
|
|
|
|
//read output
|
|
bool doValue[8]; idx = 0;
|
|
doValue[idx++] = digitalRead(PINO_GUIDEMOTOR_LDIR);
|
|
doValue[idx++] = digitalRead(PINO_GUIDEMOTOR_LRUN);
|
|
doValue[idx++] = digitalRead(PINO_GUIDEMOTOR_RDIR);
|
|
doValue[idx++] = digitalRead(PINO_GUIDEMOTOR_RRUN);
|
|
doValue[idx++] = digitalRead(PINO_EMPTY_26);
|
|
doValue[idx++] = digitalRead(PINO_EMPTY_27);
|
|
doValue[idx++] = digitalRead(PINO_EMPTY_28);
|
|
doValue[idx++] = digitalRead(PINO_EMPTY_29);
|
|
|
|
for (int i = 0; i < idx; i++)
|
|
{
|
|
bool oldValue = bitRead(var.IOData, i + 16);
|
|
bool isNewValue = false;
|
|
if (oldValue != doValue[i]) isNewValue = true;
|
|
bitWrite(var.IOData, i + 16, doValue[i]);//0번부터 기록한다
|
|
|
|
//값이 변화했따
|
|
if (isNewValue) IOValueChanged(true, i, !oldValue);
|
|
}
|
|
|
|
//Read Analog (마그넷 센서 값만 확인한다)
|
|
idx = 0;
|
|
|
|
//원본값은 그대로 저장한다
|
|
var.ANData[idx++] = analogRead(PINAI_0);
|
|
var.ANData[idx++] = analogRead(PINAI_1);
|
|
var.ANData[idx++] = analogRead(PINAI_2);
|
|
var.ANData[idx++] = analogRead(PINAI_3);
|
|
|
|
idx = 0;
|
|
var.AOData[idx++] = 0;
|
|
var.AOData[idx++] = 0;
|
|
var.AOData[idx++] = 0;
|
|
var.AOData[idx++] = 0;
|
|
|
|
//파워체크
|
|
if (IsPowerLoss() == true) var.setFlag(FLAG_POWERLOSS, true, "IOUpdate");
|
|
else var.setFlag(FLAG_POWERLOSS, false, "IOUpdate");
|
|
}
|
|
|
|
//메인전원이 OFF되어있는지 확인합니다.(리밋센서의OFF여부로 체크함)
|
|
bool IoClass::IsPowerLoss()
|
|
{
|
|
//얘들은 B접점이라서. 항상 ON 이 되어있는 애들이다.
|
|
//전원이 OFF되면 모든 센서가 LOW 상태가 된다
|
|
//하지만 신호를 반전(!) 시켜두었으므로 모두 켜진다면 OFF 된 경우이다)
|
|
bool b1 = bitRead(var.IOData, IDXI_LIMIT_LD);
|
|
bool b2 = bitRead(var.IOData, IDXI_LIMIT_LU);
|
|
bool b3 = bitRead(var.IOData, IDXI_LIMIT_RU);
|
|
bool b4 = bitRead(var.IOData, IDXI_LIMIT_RD);
|
|
if (b1 == true && b2 == true && b3 == true && b4 == true) return true;
|
|
return false;
|
|
}
|
|
|
|
//IO상태값이 변화되었을때 동작한다
|
|
void IoClass::IOValueChanged(bool isout, uint8_t index, bool newValue)
|
|
{
|
|
if (isout == false) //입력포트
|
|
{
|
|
|
|
if (index == IDXI_BTN_1)
|
|
{
|
|
if (newValue == true)
|
|
{
|
|
hmi.SendMessage("button 1 pressed", false);
|
|
}
|
|
}
|
|
if (index == IDXI_BTN_2)
|
|
{
|
|
if (newValue == true)
|
|
{
|
|
hmi.SendMessage("button 2 pressed", false);
|
|
}
|
|
}
|
|
if (index == IDXI_BTN_3)
|
|
{
|
|
if (newValue == true)
|
|
{
|
|
hmi.SendMessage("button 3 pressed", false);
|
|
}
|
|
}
|
|
if (index == IDXI_BTN_4)
|
|
{
|
|
if (newValue == true)
|
|
{
|
|
hmi.SendMessage("button 4 pressed", false);
|
|
}
|
|
}
|
|
|
|
//양쪽면의 z축 UP/DN 버튼 할당
|
|
if (index == IDXI_BTN_ZUP)
|
|
{
|
|
if (newValue == true)
|
|
{
|
|
if (mot.GetZDirL() == ZDIR_CW && mot.IsMoveZ() == true)
|
|
{
|
|
hmi.SendMessage("z-stop by h/w button", false);
|
|
mot.SetZRun(ZRUN_STOP);
|
|
}
|
|
if (mot.GetZDirL() == ZDIR_CCW && mot.IsMoveZ() == true)
|
|
{
|
|
hmi.SendMessage("z-stop by h/w button", false);
|
|
mot.SetZRun(ZRUN_STOP);
|
|
}
|
|
else {
|
|
mot.SetZRun(ZRUN_UP);
|
|
hmi.SendMessage("Z-Up by H/W Button", false);
|
|
}
|
|
|
|
}
|
|
}
|
|
else if (index == IDXI_BTN_ZDN)
|
|
{
|
|
if (newValue == true)
|
|
{
|
|
if (mot.GetZDirL() == ZDIR_CCW && mot.IsMoveZ() == true)
|
|
{
|
|
hmi.SendMessage("z-stop by h/w button", false);
|
|
mot.SetZRun(ZRUN_STOP);
|
|
}
|
|
if (mot.GetZDirL() == ZDIR_CW && mot.IsMoveZ() == true)
|
|
{
|
|
hmi.SendMessage("z-stop by h/w button", false);
|
|
mot.SetZRun(ZRUN_STOP);
|
|
}
|
|
else {
|
|
mot.SetZRun(ZRUN_DN);
|
|
hmi.SendMessage("Z-Dn by H/W Button", false);
|
|
}
|
|
}
|
|
}
|
|
else if (index == IDXI_EMG) //비상정지 혹은 범퍼센서가 감지된경우
|
|
{
|
|
if (newValue == true)
|
|
{
|
|
hmi.SendMessage("EMG Stop Detected", false);
|
|
mot.SetZRun(ZRUN_STOP);
|
|
}
|
|
}
|
|
|
|
//일반적으로 Rising 상태일때에만 동작시킨다.
|
|
if (newValue == true)
|
|
{
|
|
//하단센서2개가 들어왔다면 FLAG를 설정한다
|
|
if (index == IDXI_LIMIT_LD || index == IDXI_LIMIT_RD)
|
|
{
|
|
if (index == IDXI_LIMIT_LD )
|
|
{
|
|
if (var.getFlag(FLAG_LIMITLOWL) == false)
|
|
{
|
|
var.setFlag(FLAG_LIMITLOWL, true, "IO:Limit Sensor(LD) Changed");
|
|
hmi.SendMessage(F("SET:FLAG:LOW_LIMITL:1"), false);
|
|
}
|
|
}
|
|
else if (index == IDXI_LIMIT_RD )
|
|
{
|
|
if (var.getFlag(FLAG_LIMITLOWR) == false)
|
|
{
|
|
var.setFlag(FLAG_LIMITLOWR, true, "IO:Limit Sensor(RD) Changed");
|
|
hmi.SendMessage(F("SET:FLAG:LOW_LIMITR:1"), false);
|
|
}
|
|
}
|
|
else {
|
|
bool b1 = bitRead(var.IOData, IDXI_LIMIT_LD);
|
|
bool b2 = bitRead(var.IOData, IDXI_LIMIT_RD);
|
|
if (b1 == false && var.getFlag(FLAG_LIMITLOWL) == true)
|
|
{
|
|
var.setFlag(FLAG_LIMITLOWL, false, "IO:Limit Sensor(LD) Changed");
|
|
hmi.SendMessage(F("SET:FLAG:LOW_LIMITL:0"), false);
|
|
}
|
|
if (b2 == false && var.getFlag(FLAG_LIMITLOWR) == true)
|
|
{
|
|
var.setFlag(FLAG_LIMITLOWR, false, "IO:Limit Sensor(RD) Changed");
|
|
hmi.SendMessage(F("SET:FLAG:LOW_LIMITR:0"), false);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (index == IDXI_LIMIT_LU || index == IDXI_LIMIT_RU)
|
|
{
|
|
|
|
|
|
if (index == IDXI_LIMIT_LU || var.getFlag(FLAG_LIMITHIGHL) == false)
|
|
{
|
|
var.setFlag(FLAG_LIMITHIGHL, true, "IO:Limit Sensor(LU) Changed");
|
|
hmi.SendMessage(F("SET:FLAG:HIGH_LIMITL:1"), false);
|
|
}
|
|
else if (index == IDXI_LIMIT_RU || var.getFlag(FLAG_LIMITHIGHR) == false)
|
|
{
|
|
var.setFlag(FLAG_LIMITHIGHR, true, "IO:Limit Sensor(RU) Changed");
|
|
hmi.SendMessage(F("SET:FLAG:HIGH_LIMITR:1"), false);
|
|
}
|
|
else {
|
|
bool b1 = bitRead(var.IOData, IDXI_LIMIT_LU);
|
|
bool b2 = bitRead(var.IOData, IDXI_LIMIT_RU);
|
|
if (b1 == false && var.getFlag(FLAG_LIMITHIGHL) == true)
|
|
{
|
|
var.setFlag(FLAG_LIMITHIGHL, false, "IO:Limit Sensor(LU) Changed");
|
|
hmi.SendMessage(F("SET:FLAG:HIGH_LIMITL:0"), false);
|
|
}
|
|
if (b2 == false && var.getFlag(FLAG_LIMITHIGHR) == true)
|
|
{
|
|
var.setFlag(FLAG_LIMITHIGHR, false, "IO:Limit Sensor(RU) Changed");
|
|
hmi.SendMessage(F("SET:FLAG:HIGH_LIMITR:0"), false);
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
//오버로드 플래그 활성화
|
|
if (index == IDXI_OVERLOADL)
|
|
{
|
|
mot.SetZRun(ZRUN_STOP);
|
|
hmi.SendMessage(F("All Mot Stop by OverloadL"), false);
|
|
}
|
|
else if (index == IDXI_OVERLOADR)
|
|
{
|
|
mot.SetZRun(ZRUN_STOP);
|
|
hmi.SendMessage(F("All Mot Stop by OverloadR"), false);
|
|
}
|
|
|
|
}
|
|
else //해당값이 OFF 되었다
|
|
{
|
|
|
|
//대상이 리밋센서라면 플래그를 설정 해준다
|
|
if (index == IDXI_LIMIT_LD)
|
|
{
|
|
|
|
hmi.SendMessage("Limit Sensor(LD) Change " + String(index) + "=" + String(newValue), false);
|
|
if (var.getFlag(FLAG_LIMITLOWL) == true)
|
|
{
|
|
var.setFlag(FLAG_LIMITLOWL, false, "IO:Limit Sensor Changed(LD)");
|
|
hmi.SendMessage(F("SET:FLAG:LOW_LIMIT:0"), false);
|
|
}
|
|
}
|
|
else if (index == IDXI_LIMIT_RD)
|
|
{
|
|
|
|
hmi.SendMessage("Limit Sensor(RD) Change " + String(index) + "=" + String(newValue), false);
|
|
if (var.getFlag(FLAG_LIMITLOWR) == true)
|
|
{
|
|
var.setFlag(FLAG_LIMITLOWR, false, "IO:Limit Sensor Changed(RD)");
|
|
hmi.SendMessage(F("SET:FLAG:LOW_LIMIT:0"), false);
|
|
}
|
|
}
|
|
else if (index == IDXI_LIMIT_LU )
|
|
{
|
|
hmi.SendMessage("Limit Sensor(LU) Change " + String(index) + "=" + String(newValue), false);
|
|
if (var.getFlag(FLAG_LIMITHIGHL) == true)
|
|
{
|
|
var.setFlag(FLAG_LIMITHIGHL, false, "IO:Limit Sensor Changed(LU)");
|
|
hmi.SendMessage(F("SET:FLAG:HIGH_LIMIT:0"), false);
|
|
}
|
|
}
|
|
else if (index == IDXI_LIMIT_RU)
|
|
{
|
|
hmi.SendMessage("Limit Sensor(RU) Change " + String(index) + "=" + String(newValue), false);
|
|
if (var.getFlag(FLAG_LIMITHIGHR) == true)
|
|
{
|
|
var.setFlag(FLAG_LIMITHIGHR, false, "IO:Limit Sensor Changed(RU)");
|
|
hmi.SendMessage(F("SET:FLAG:HIGH_LIMIT:0"), false);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else //출력포트
|
|
{
|
|
//일반적으로 Rising 상태일때에만 동작시킨다.
|
|
if (newValue == true)
|
|
{
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
IoClass io;
|