initial commit
This commit is contained in:
chi
256
Arduino_PLC/motor.cpp
Normal file
256
Arduino_PLC/motor.cpp
Normal file
@@ -0,0 +1,256 @@
|
||||
#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;
|
||||
Reference in New Issue
Block a user