257 lines
6.9 KiB
C++
257 lines
6.9 KiB
C++
#include "motor.h"
|
|
#include "IO.h"
|
|
#include "VarClass.h"
|
|
#include "UtilClass.h"
|
|
#include "HmiClass.h"
|
|
|
|
void motor::Setup()
|
|
{
|
|
hmi.SendMessage("##:MOT:SETUP", false);
|
|
|
|
//Z축 멈춤
|
|
SetZRun(ZRUN_STOP);
|
|
}
|
|
void motor::Update()
|
|
{
|
|
//##########################
|
|
// 사용자추가코드
|
|
//##########################
|
|
|
|
if (var.pingtime == 0 || var.pingtime > millis()) var.pingtime = millis();
|
|
long runtime = millis() - var.pingtime;
|
|
|
|
//중지조건 체크
|
|
//(충전중, 비상정지, 오버로드, 설정화면)
|
|
bool bStop1 = var.getFlag(FLAG_SETUP);
|
|
bool bStop2L = bitRead(var.IOData, IDXI_OVERLOADL);
|
|
bool bStop2R = bitRead(var.IOData, IDXI_OVERLOADR);
|
|
bool bStop3 = bitRead(var.IOData, IDXI_EMG);
|
|
bool bStop4 = bitRead(var.IOData, IDXI_STOP);
|
|
|
|
//중지조건이 활성화되었다면 처리
|
|
String stpReason = "";
|
|
|
|
//Z축 중지는 오버로드만 확읺ㄴ다.
|
|
if (bStop3== true)
|
|
{
|
|
if (var.getFlag(FLAG_STOPZ) == false)
|
|
{
|
|
dbgSerial.println("Set Flag On: flag_stopz by EMG");
|
|
var.setFlag(FLAG_STOPZ, true, "MotUpdate");
|
|
}
|
|
}
|
|
else if (bStop2L == true)
|
|
{
|
|
if (var.getFlag(FLAG_STOPZ) == false)
|
|
{
|
|
dbgSerial.println("Set Flag On: flag_stopz by OverloadL");
|
|
var.setFlag(FLAG_STOPZ, true, "MotUpdate_OVLL");
|
|
}
|
|
}
|
|
else if (bStop2R == true)
|
|
{
|
|
if (var.getFlag(FLAG_STOPZ) == false)
|
|
{
|
|
dbgSerial.println("Set Flag On: flag_stopz by OverloadR");
|
|
var.setFlag(FLAG_STOPZ, true, "MotUpdate_OVLR");
|
|
}
|
|
}
|
|
else if (var.getFlag(FLAG_STOPZ) == true)
|
|
{
|
|
dbgSerial.println("Set Flag Off");
|
|
var.setFlag(FLAG_STOPZ, false, "MotUpdate");
|
|
}
|
|
|
|
//가이드 모터 동작시 Limit 센서 체크
|
|
ZLimit_AutoStop();
|
|
|
|
if (var.getFlag(FLAG_STOPZ) == true)
|
|
{
|
|
if (IsMoveZ() == true)
|
|
{
|
|
Serial.println("Z-Axis Auto Stop by 'STOP-Z' Condition");
|
|
mot.SetZRun(ZRUN_STOP);
|
|
}
|
|
}
|
|
|
|
//Serial.println("motor update");
|
|
}
|
|
|
|
|
|
//Z축이 동작죽이라면 리밋센서값을 보고 자동으로 멈춥니다
|
|
void motor::ZLimit_AutoStop()
|
|
{
|
|
if (mot.IsMoveZ() == true)
|
|
{
|
|
//정방향이동시(올라간다)
|
|
if (mot.GetZDirL() == ZDIR_CW)
|
|
{
|
|
if (bitRead(var.IOData, IDXI_OVERLOADL) == true ) //오버로드 활성화시 멈춤
|
|
{
|
|
mot.SetZRun(ZRUN_STOP);
|
|
if (var.getFlag(FLAG_SETUP) == false) hmi.SendMessage("Z-Axis Auto Stop by OverloadL", false);
|
|
}
|
|
else if (bitRead(var.IOData, IDXI_OVERLOADR) == true ) //오버로드 활성화시 멈춤
|
|
{
|
|
mot.SetZRun(ZRUN_STOP);
|
|
if (var.getFlag(FLAG_SETUP) == false) hmi.SendMessage("Z-Axis Auto Stop by OverloadR", false);
|
|
}
|
|
else {
|
|
//상단센서가 하나라도 동작하면 멈춘다
|
|
if (mot.IsMoveZL() == true && bitRead(var.IOData, IDXI_LIMIT_LU) == true)
|
|
{
|
|
mot.SetZRunL(false);
|
|
if (var.getFlag(FLAG_SETUP) == false) hmi.SendMessage("Z-Axis (Left) Auto Stop by Upper Limit", false);
|
|
}
|
|
if (mot.IsMoveZR() == true && bitRead(var.IOData, IDXI_LIMIT_RU) == true)
|
|
{
|
|
mot.SetZRunR(false);
|
|
if (var.getFlag(FLAG_SETUP) == false)hmi.SendMessage("Z-Axis (Right) Auto Stop by Upper Limit", false);
|
|
}
|
|
}
|
|
}
|
|
else //역방향이동(내려간다)
|
|
{
|
|
//하단센서가 하나라도 동작하면 멈춘다
|
|
if (mot.IsMoveZL() == true && bitRead(var.IOData, IDXI_LIMIT_LD) == true)
|
|
{
|
|
mot.SetZRunL(false);
|
|
if (var.getFlag(FLAG_SETUP) == false)hmi.SendMessage("Z-Axis (Left) Auto Stop by Lower Limit", false);
|
|
}
|
|
if (mot.IsMoveZR() == true && bitRead(var.IOData, IDXI_LIMIT_RD) == true)
|
|
{
|
|
mot.SetZRunR(false);
|
|
if (var.getFlag(FLAG_SETUP) == false)hmi.SendMessage("Z-Axis (Right) Auto Stop by Lower Limit", false);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
//가이드 모터 진행방향을 반환(CW:Up, CCW:Down)
|
|
ZDirection motor::GetZDirL()
|
|
{
|
|
bool value = bitRead(var.IOData, IDXO_GUIDEMOTOR_LDIR + 16);
|
|
if (value == false) return ZDIR_CW;
|
|
else return ZDIR_CCW;
|
|
}
|
|
ZDirection motor::GetZDirR()
|
|
{
|
|
bool value = bitRead(var.IOData, IDXO_GUIDEMOTOR_RDIR + 16);
|
|
if (value == false) return ZDIR_CW;
|
|
else return ZDIR_CCW;
|
|
}
|
|
|
|
//가이드 모터(좌+우) 동작 여부
|
|
bool motor::IsMoveZ()
|
|
{
|
|
if (IsMoveZL() == true) return true;
|
|
else if (IsMoveZR() == true) return true;
|
|
else return false;
|
|
}
|
|
|
|
//가이드 모터(좌) 동작 여부
|
|
bool motor::IsMoveZL()
|
|
{
|
|
bool v1 = bitRead(var.IOData, IDXO_GUIDEMOTOR_LRUN + 16);
|
|
//bool v2 = bitRead(var.IOData, IDXO_GUIDEMOTOR_LEFT + 16);
|
|
return v1 ;
|
|
}
|
|
|
|
//가이드 모터(우) 동작 여부
|
|
bool motor::IsMoveZR()
|
|
{
|
|
bool v1 = bitRead(var.IOData, IDXO_GUIDEMOTOR_RRUN + 16);
|
|
//bool v2 = bitRead(var.IOData, IDXO_GUIDEMOTOR_RIGHT + 16);
|
|
return v1 ;
|
|
}
|
|
|
|
//가이드모터 진행방향 결정(True:정방향, False:역방향)
|
|
void motor::SetZDir(ZDirection direction)
|
|
{
|
|
if (direction == ZDIR_CW) {
|
|
digitalWrite(PINO_GUIDEMOTOR_LDIR, LOW);
|
|
digitalWrite(PINO_GUIDEMOTOR_RDIR, LOW);
|
|
}
|
|
else {
|
|
digitalWrite(PINO_GUIDEMOTOR_LDIR, HIGH);
|
|
digitalWrite(PINO_GUIDEMOTOR_RDIR, HIGH);
|
|
}
|
|
}
|
|
|
|
void motor::SetZRun(ZRunMode runMode) {
|
|
|
|
bool bEmg = var.getFlag(FLAG_STOPZ); //Z축이 움직이면 안되는 조건이므로 처리하지 않는다.
|
|
|
|
if (runMode == ZRUN_UP)
|
|
{
|
|
if (bEmg) {
|
|
hmi.SendMessage("[Ignore] Z-Run by Stop-Z Flag", true);
|
|
return;
|
|
}
|
|
SetZDir(ZDIR_CW);
|
|
SetZRunL(true);
|
|
SetZRunR(true);
|
|
if (var.getFlag(FLAG_SETUP) == false) hmi.SendMessage("Guide Up", false);
|
|
}
|
|
else if (runMode == ZRUN_DN)
|
|
{
|
|
if (bEmg) {
|
|
hmi.SendMessage("[Ignore] Run by Stop-Z Flag", true);
|
|
return;
|
|
}
|
|
SetZDir(ZDIR_CCW);
|
|
SetZRunL(true);
|
|
SetZRunR(true);
|
|
if (var.getFlag(FLAG_SETUP) == false) hmi.SendMessage("Guide Dn", false);
|
|
}
|
|
else if (runMode == ZRUN_STOP)
|
|
{
|
|
SetZRunL(false);
|
|
SetZRunR(false);
|
|
if (var.getFlag(FLAG_SETUP) == false) hmi.SendMessage("Guid Stop", false);
|
|
//digitalWrite(PINO_GUIDEMOTOR_RUN, LOW);
|
|
}
|
|
}
|
|
|
|
void motor::SetZRunL(bool run) {
|
|
if (run == true)
|
|
{
|
|
//if (var.getFlag(FLAG_SETUP) == false) hmi.SendMessage("Z-Axis Left Run", false);
|
|
digitalWrite(PINO_GUIDEMOTOR_LRUN, HIGH);
|
|
//digitalWrite(PINO_GUIDEMOTOR_RUN, HIGH);
|
|
}
|
|
else {
|
|
digitalWrite(PINO_GUIDEMOTOR_LRUN, LOW);
|
|
//L,R둘다 꺼져있다면 STA도 끈다
|
|
//if (bitRead(var.IOData, IDXO_GUIDEMOTOR_RIGHT + 16) == false)
|
|
//{
|
|
// if (var.getFlag(FLAG_SETUP) == false)hmi.SendMessage("Z-Axis Sta Off by Right", false);
|
|
// digitalWrite(PINO_GUIDEMOTOR_RUN, LOW);
|
|
//}
|
|
}
|
|
|
|
}
|
|
|
|
void motor::SetZRunR(bool run) {
|
|
if (run == true)
|
|
{
|
|
//if (var.getFlag(FLAG_SETUP) == false) hmi.SendMessage("Z-Axis Right Run", false);
|
|
digitalWrite(PINO_GUIDEMOTOR_RRUN, HIGH);
|
|
//digitalWrite(PINO_GUIDEMOTOR_RUN, HIGH);
|
|
}
|
|
else {
|
|
digitalWrite(PINO_GUIDEMOTOR_RRUN, LOW);
|
|
//L,R둘다 꺼져있다면 STA도 끈다
|
|
//if (bitRead(var.IOData, IDXO_GUIDEMOTOR_LEFT + 16) == false)
|
|
//{
|
|
// if (var.getFlag(FLAG_SETUP) == false) hmi.SendMessage("Z-Axis Sta Off by Right", false);
|
|
// digitalWrite(PINO_GUIDEMOTOR_RUN, LOW);
|
|
//}
|
|
}
|
|
}
|
|
|
|
|
|
motor mot;
|