Jaeyeol Lee
200117StepMotorControl
Dependencies: X_NUCLEO_IHM02A1 TextLCD
main.cpp
- Committer:
- danjecsr
- Date:
- 2020-01-17
- Revision:
- 29:d5c13978c8e9
- Parent:
- 28:ed18e436f437
File content as of revision 29:d5c13978c8e9:
/**
******************************************************************************
이프로그램은 16x2크기의 LCD표시 및 제어, 스텝모터 제어 2가지 기능을 한다.
사용자에서 실시간으로 LCD와 스텝모터의 동시제어를 위하여 2개의 쓰레드로 분리시켰다.
모터가 회전할때는 적어도 1~2초의 시간이 소모되는데, 싱글 쓰레드로 한다면 모터가 작동할때
LCD제어는 멈추게 된다. 따라서 작은시간을 서로 쪼개어 사용하여 서로간의 제어에
영향을 받지 않도록 하기 위하여 2개의 쓰레드로 분리 시켰음.
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
// mbed의 기능을 사용하기 위한 헤더파일
#include "mbed.h"
#include "rtos.h"
//텍스트LCD를 사용하기 위한 라이브러리의 헤더파일
#include "TextLCD.h"
//내부 FLASH ROM을 사용하기 위한 라이브러리 헤더파일
#include "eeprom.h"
#include <ctype.h>
//스텝모터 드라이버와 SPI통신을 위한 헤더파일
#include "DevSPI.h"
//스텝모터 확장보드 사용을 위한 라이브러리 헤더파일
#include "XNucleoIHM02A1.h"
/*----------------------------------------------LCD제어관련 변수정의 -------------------------------------------------------*/
//16x2 크기의 LCD를 제어하기 위한 객체생성
TextLCD lcd(D10, D9, D8, D7, D6, D5,TextLCD::LCD16x2); // H/L Register(RS) 선택핀, Enable(E) 선택핀, D4,D5,D6,D7(LCD출력핀)
//DigitalOut rw(D7); // H/L Read,Write신호핀
//Digital 입력핀 설정(기본으로 신호가 없을시 풀다운신호(0V)를 출력하도록 설정한다.)
DigitalIn setin(A3,PullDown); //설정버튼
DigitalIn movin(D15,PullDown); //자릿수선택 버튼
DigitalIn upin(D14,PullDown); //값올림 버튼
DigitalIn downin(D2,PullDown); //값내림 버튼
//LCD 출력의 현재시각 측정(단위:초)
time_t NOWLCD;
//LCD 출력을 조절하기 위한 이전시각(NOWLCD)저장
time_t DISPLAY_TIME;
float SV = 10.5; //절삭유 농도 지시값(기준값)
int SV1 = 1; //화면에 표시될 절삭유 농도 지시값(10의 자릿수값 정수값1문자)
int SV2 = 0; //화면에 표시될 절삭유 농도 지시값(1의 자릿수값 정수값1문자)
int SV3 = 5; //화면에 표시될 절삭유 농도 지시값(소수점1의 자릿수값 정수값1문자)
int SET_SV1 = 0; //설정모드에서 표시될 농도 지시값(10의 자릿수값)
int SET_SV2 = 0; //설정모드에서 표시될 농도 지시값(1의 자릿수값)
int SET_SV3 = 0; //설정모드에서 표시될 농도 지시값(소수점1의 자릿수값)
//lcd설정값과 밸브의 현재위치를 저장하기 위한 플래쉬메모리의 위치(LCD설정값(0x0000,0x0001), 밸브의 현재위치(0x0002,0x0003)(오일밸브현재 위치,희석수밸브현재위치
uint16_t VirtAddVarTab[4] = {0x0000, 0x0001, 0x0002, 0x0003};
//lcd설정값(자연수,소수점,절삭유현재위치,희석수현재위치)
uint16_t VarDataTab[] = {0, 0, 0, 0};
float PV = 0; //1초동안 샘플링한 절삭유 농도값
float PPV = 0; //이전측정 절삭유 농도값
float CPV = 0; //현재 절삭유 농도값
float SPV = 0; //1초동안 계속 평균을 계산하여 저장하는 절삭유 농도값
int OIL_VALVE_OPEN_P = 0; //오일밸브 열림%
int WATER_VALVE_OPEN_P = 0; //희석수밸브 열림%
int OIL_VALVE_OPEN_PP = 0; //오일밸브 열림%(이전값)
int WATER_VALVE_OPEN_PP = 0; //희석수밸브 열림%(이전값)
bool set_button = false; //설정버튼 눌림 플래그
bool mov_button = false; //자릿수선택 버튼 눌림 플래그
bool up_button = false; //값올림버튼 눌림 플래그
bool down_button = false; //값내림버튼 눌림 플래그
bool set_mode = false; //설정버튼 ON/OFF모드
int cursor_loc = 0; //SV값을 설정하기 위하여 현재cursor의 위치를 저장(2번째행 11번열, 12번열 두개값중에 하나를 저장하고 있음)
/*----------------------------------------------STEP모터 제어관련 변수정의 -------------------------------------------------------*/
//스텝수 정의
#define STEPS_1 (200 * 128) //1바퀴에 200스텝(1스텝당 1.8도)을 가지고 있는 스텝모터를 1/128정밀도의 마이크로 스텝을 제어.(1바퀴 회전정의)
#define STEPS_2 (STEPS_1 * 2) //2바퀴 회전정의
//스텝모터 확장모듈 드라이버 객체생성
XNucleoIHM02A1 *x_nucleo_ihm02a1;
//스텝모터 파라미터값 정의
L6470_init_t init[L6470DAISYCHAINSIZE] = {
// 첫번째 Motor.(절삭유밸브 모터)
{
12.0, /* Motor supply voltage in V. */
200, /* Min number of steps per revolution for the motor. */
1.7, /* Max motor phase voltage in A. */
3.06, /* Max motor phase voltage in V. */
300.0, /* Motor initial speed [step/s]. */
500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
992.0, /* Motor maximum speed [step/s]. */
0.0, /* Motor minimum speed [step/s]. */
602.7, /* Motor full-step speed threshold [step/s]. */
3.06, /* Holding kval [V]. */
3.06, /* Constant speed kval [V]. */
3.06, /* Acceleration starting kval [V]. */
3.06, /* Deceleration starting kval [V]. */
61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
392.1569e-6, /* Start slope [s/step]. */
643.1372e-6, /* Acceleration final slope [s/step]. */
643.1372e-6, /* Deceleration final slope [s/step]. */
0, /* Thermal compensation factor (range [0, 15]). */
3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
0xFF, /* Alarm conditions enable. */
0x2E88 /* Ic configuration. */
},
//두번째 Motor(희석수밸브 모터)
{
12.0, /* Motor supply voltage in V. */
200, /* Min number of steps per revolution for the motor. */
1.7, /* Max motor phase voltage in A. */
3.06, /* Max motor phase voltage in V. */
300.0, /* Motor initial speed [step/s]. */
500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
992.0, /* Motor maximum speed [step/s]. */
0.0, /* Motor minimum speed [step/s]. */
602.7, /* Motor full-step speed threshold [step/s]. */
3.06, /* Holding kval [V]. */
3.06, /* Constant speed kval [V]. */
3.06, /* Acceleration starting kval [V]. */
3.06, /* Deceleration starting kval [V]. */
61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
392.1569e-6, /* Start slope [s/step]. */
643.1372e-6, /* Acceleration final slope [s/step]. */
643.1372e-6, /* Deceleration final slope [s/step]. */
0, /* Thermal compensation factor (range [0, 15]). */
3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
0xFF, /* Alarm conditions enable. */
0x2E88 /* Ic configuration. */
}
};
//현재 농도를 받아오는 Analog input 핀번호를 선택한다.(PA_0)즉 핀번호 Analog 0번핀을 선택한다.
AnalogIn CurCon(A0);
//절삭유밸브와 희석수밸브의 초기위치
int oilhomeposition = 0;
int waterhomeposition = 0;
//샘플링 카운트 수
int samplecnt = 0;
//절삭유밸브와 희석수밸브의 현재 포지션
short oilcurrposition = 0;
short watercurrposition = 0;
//오일탱크밸브와 수탱크밸브가 오픈된 시간을 저장
Timer OIL_VALVE_TIMER;
Timer WATER_VALVE_TIMER;
//밸브가 열린후 카운트 시간
time_t OIL_VALVE_OPEN_TIME;
time_t WATER_VALVE_OPEN_TIME;
//현재시각
time_t NOW;
//농도값 샘플링을 위한 시각비교값 저장
time_t SAMPLE;
//오일탱크밸브와 수탱크밸브가 열려있는지 저장
bool OIL_VALVE_ON = false;
bool WATER_VALVE_ON = false;
//딜레이시간 정의 (milli seconds)
#define DELAY_1 1000
#define DELAY_2 2000
#define DELAY_3 5000
//LCD제어를 위한 쓰레드 생성
void LCD_CTRL_THREAD(void const *args)
{
////설정값을 FLASH메모리에서 읽고쓰기하기 위하여 락을 해제한다.
//HAL_FLASH_Unlock();
//EE_Init();
//SV값은 총4byte float값인데 EEPROM Library는 기본적으로 2byte씩메모리를 엑세스하게 되어있음
//따라서 2byte메모리를 읽어서 4byte값으로 만들것임.
for (int i=0; i<2; i++)
{
EE_ReadVariable(VirtAddVarTab[i], &VarDataTab[i]);
}
//읽은값을 정수자릿수 2byte + 소숫점1자릿수 2byte값을 합쳐서 4byte float값으로 변경(설정값 저장)
SV = VarDataTab[0] + VarDataTab[1]*0.1;
//SV값을 LCD에 출력하기위하여 SV1,SV2,SV3로 분리한다.
SV1 = (int)(SV/10);
SV2 = (int)(SV-(SV1*10));
SV3 = (int)(((SV-((SV1*10)+SV2))*10)+0.5);
//rw =0;
lcd.cls(); //화면을 초기화 한다.
//절삭유 공급밸브 열림% LCD출력
lcd.locate(0,0);
lcd.printf("P1:");
lcd.printf("%03d%%",OIL_VALVE_OPEN_P);
//절삭유 현재 농도값 LCD출력
lcd.locate(8,0);
lcd.printf("PV:");
lcd.printf("%02.1f%%",PV);
//희석수 공급밸브 열림% LCD출력
lcd.locate(0,1);
lcd.printf("P2:");
lcd.printf("%03d%%",WATER_VALVE_OPEN_P);
//절삭유 지시(목표) 농도값 LCD출력
lcd.locate(8,1);
lcd.printf("SV:");
lcd.printf("%d%d.%d%%",SV1,SV2,SV3);
//커서표시와 깜밖임을 없앤다.
lcd.setCursor(lcd.CurOff_BlkOff);
while (1)
{
NOWLCD = time(NULL);
/*------------------------------- LCD표시 ---------------------------------------------------*/
//1.5초마다 화면을 갱신한다.
if(NOWLCD - DISPLAY_TIME >= 1 and !set_mode)
{
//현재 측정한 절삭유 농도값이 이전측정값과 다르다면 LCD화면을 갱신한다.
if(PV != PPV)
{
lcd.locate(11,0);
lcd.printf("%02.1f%%",PV);
//현재값을 이전값변수에 입력한다.(다음타이밍에 비교하기 위하여)
PPV = PV;
}
//현재 오일밸브의 오픈%가 이전값과 달다면 LCD화면을 갱신한다.
if(OIL_VALVE_OPEN_P != OIL_VALVE_OPEN_PP)
{
lcd.locate(3,0);
lcd.printf("%03d%%",OIL_VALVE_OPEN_P);
//현재값을 이전 측정값을 저장하는 변수에 복사
OIL_VALVE_OPEN_PP = OIL_VALVE_OPEN_P;
}
//현재 희석수밸브의 오픈%가 이전값과 달다면 LCD화면을 갱신한다.
if(WATER_VALVE_OPEN_P != WATER_VALVE_OPEN_PP)
{
lcd.locate(3,1);
lcd.printf("%03d%%",WATER_VALVE_OPEN_P);
//현재값을 이전 측정값을 저장하는 변수에 복사
WATER_VALVE_OPEN_PP = WATER_VALVE_OPEN_P;
}
//다음 시간비교를 위해서 현재시각을 저장한다.
DISPLAY_TIME = NOWLCD;
}
/*------------------------------- LCD설정 ---------------------------------------------------*/
//----------농도 설정
//SV(농도지시값)값 설정버튼(SET)을 눌렀을경우 플래그를 세팅한다.(정확한 세팅상태를 지정하기 위함)
if(setin and set_button == false)
{
set_button = true; //설정버튼 플래그 활성화
}
//SET버튼이 눌려지다가 떨어지면 세팅모드로 들어간다.
if(!setin and set_button == true and set_mode == false)
{
printf("LCD SETTING MODE IN\r\n");
lcd.locate(14,1); //설정값 첫번째 위치로 이동
cursor_loc = 14; //현재 커서위치 저장
lcd.setCursor(lcd.CurOff_BlkOn); //설정위치에서 커서 깜박임
set_mode = true; //설정모드 활성화
set_button = false; //설정버튼 플래그 초기화
}
//만약 SV설정 버튼이 true인 상태에서 한번더 누르면 현재값을 저장하고 모드를 빠져나온다(커서의 깜박임을 멈춘다.)
if(!setin and set_button == true and set_mode == true )
{
printf("LCD SETTING MODE OUT\r\n");
SV = (SV1*10) + (SV2*1) + (SV3*0.1); //설정값을 float형(4byte 부동소수점형)으로 저장한다.
//플래쉬메모리에 설정값을 저장한다.
//정수자릿수와 소수점 자릿수로 나눈다.
VarDataTab[0] = (uint16_t)SV; //정수자릿수
VarDataTab[1] = (uint16_t)(((SV-VarDataTab[0])*10)+0.5); //소수점1자릿수(45.5라는 숫자가 45.4999999같은 값이 될수 있으므로 소수점 첫번째에서 반올림한다.)
//내부 플래쉬 메모리에 값을 저장한다.
EE_WriteVariable(VirtAddVarTab[0],VarDataTab[0] );
EE_WriteVariable(VirtAddVarTab[1],VarDataTab[1] );
lcd.setCursor(lcd.CurOff_BlkOff); //설정위치의 커버 깜박임 꺼짐
set_mode = false; //설정모드 종료
set_button = false; //설정버튼 플래그 초기화
}
//-------------자릿수 설정
//자릿수 지정버튼(◁)을 눌렀을경우 플래그를 세팅한다.(정확한 세팅상태를 지정하기 위함)
if(movin and mov_button == false and set_mode == true)
{
mov_button = true; //자릿수 이동버튼 플래그 활성화
}
//만약 현재 설정모드이고 자릿수 버튼이 눌려졌다면 커서를 앞뒤로 이동한다.
if(!movin and mov_button == true and set_mode == true)
{
printf("LCD MOVE\r\n");
//만약 현재 cursor의 위치가 11이라면 14로 옮기고, 12라면 11로, 14라면 12로 옮긴다.
if(cursor_loc == 11)
{
//커서의 위치를 옮긴후 현재의 위치를 저장한다.
lcd.locate(14,1);
cursor_loc = 14;
}
//만약 현재 cursor의 위치가 12라면 11로 옮긴다.
else if(cursor_loc == 12)
{
//커서의 위치를 옮긴후 현재의 위치를 저장한다.
lcd.locate(11,1);
cursor_loc = 11;
}
//만약 현재 cursor의 위치가 14라면 12로 옮긴다.
else if(cursor_loc == 14)
{
//커서의 위치를 옮긴후 현재의 위치를 저장한다.
lcd.locate(12,1);
cursor_loc = 12;
}
mov_button = false; //자릿수 이동버튼 플래그 초기화
}
//------------지시값 설정
//설정모드에서 값올림(△)버튼을 눌렀을 경우 올림버튼 플래그를 활성화 한다.
if(upin and up_button == false and set_mode == true)
{
up_button = true; //값올림 버튼 플래그 활성화
}
//설정모드에서 값올림 버튼을 눌렀다면 현재 커서가 깜박이는 위치의 자릿수 값을 1씩 올린다.
if(!upin and up_button == true and set_mode == true)
{
printf("LCD UP\r\n");
//10의 자릿수에 커서가 위치해 있다면 10의자릿수 값에 1을 더한다.
if(cursor_loc == 11)
{
SV1++;
//만약 더한값이 10이상이라면 다시 0으로 바꾼다.
if(SV1 >= 10) SV1 = 0;
//LCD화면의 값을 바꾼다.
lcd.printf("%d",SV1);
//화면에 프린트하면 location이 자동으로 증가하므로 다시 원래위치로 돌린다.
lcd.locate(11,1);
cursor_loc == 11;
}
//1의 자릿수에 커서가 위치해 있다면 1의자릿수 값에 1을 더한다.
else if(cursor_loc == 12)
{
SV2++;
//만약 더한값이 10이상이라면 다시 0으로 바꾼다.
if(SV2 >= 10) SV2 = 0;
//LCD화면의 값을 바꾼다.
lcd.printf("%d",SV2);
//화면에 프린트하면 location이 자동으로 증가하므로 다시 원래위치로 돌린다.
lcd.locate(12,1);
cursor_loc == 12;
}
//소수점1의 자릿수에 커서가 위치해 있다면 소수점1의자릿수 값에 1을 더한다.
else if(cursor_loc == 14)
{
SV3++;
//만약 더한값이 10이상이라면 다시 0으로 바꾼다.
if(SV3 >= 10) SV3 = 0;
//LCD화면의 값을 바꾼다.
lcd.printf("%d",SV3);
//화면에 프린트하면 location이 자동으로 증가하므로 다시 원래위치로 돌린다.
lcd.locate(14,1);
cursor_loc == 14;
}
up_button = false; //값올림 버튼 플래그 초기화
}
//설정모드에서 값내림(▽)버튼을 눌렀을 경우 내림버튼 플래그를 활성화 한다.
if(downin and down_button == false and set_mode == true)
{
down_button = true; //값내림 버튼 플래그 활성화
}
//설정모드에서 값내림 버튼을 눌렀다면 현재 커서가 깜박이는 위치의 자릿수 값을 1씩 내린다.
if(!downin and down_button == true and set_mode == true)
{
printf("LCD DOWN\r\n");
//10의 자릿수에 커서가 위치해 있다면 10의자릿수 값에 1을 뺀다.
if(cursor_loc == 11)
{
SV1--;
//만약 뺀값이 -1이하라면 값을 9로 바꾼다.
if(SV1 <= -1) SV1 = 9;
//LCD화면의 값을 바꾼다.
lcd.printf("%d",SV1);
//화면에 프린트하면 location이 자동으로 증가하므로 다시 원래위치로 돌린다.
lcd.locate(11,1);
cursor_loc == 11;
}
//1의 자릿수에 커서가 위치해 있다면 1의자릿수 값에 1을 뺀다
else if(cursor_loc == 12)
{
SV2--;
//만약 뺀값이 -1이하라면 값을 9로 바꾼다.
if(SV2 <= -1) SV2 = 9;
//LCD화면의 값을 바꾼다.
lcd.printf("%d",SV2);
//화면에 프린트하면 location이 자동으로 증가하므로 다시 원래위치로 돌린다.
lcd.locate(12,1);
cursor_loc == 12;
}
//소수점1의 자릿수에 커서가 위치해 있다면 소수점1의자릿수 값에 1을 뺀다
else if(cursor_loc == 14)
{
SV3--;
//만약 뺀값이 10이하라면 값을 9로 바꾼다.
if(SV3 <= -1) SV3 = 9;
//LCD화면의 값을 바꾼다.
lcd.printf("%d",SV3);
//화면에 프린트하면 location이 자동으로 증가하므로 다시 원래위치로 돌린다.
lcd.locate(14,1);
cursor_loc == 14;
}
down_button = false; //값내림 버튼 플래그 초기화
}
//쓰레드 대기
Thread::wait(10);
}
}
/* Main ----------------------------------------------------------------------*/
int main()
{
//설정값을 FLASH메모리에서 읽고쓰기하기 위하여 락을 해제한다.
HAL_FLASH_Unlock();
EE_Init();
//LCD제어를 위한 쓰레드를 호출
Thread thread(LCD_CTRL_THREAD);
//스텝모터 드라이브와 F401RE보드와 통신하기 위하여 SPI통신핀을 초기화 해준다.
#ifdef TARGET_STM32F429
DevSPI dev_spi(D11, D12, D13);
#else
DevSPI dev_spi(D11, D12, D3);
#endif
//확장보드 초기화
x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
//모터 컨트롤 모터리스트 객체
L6470 **motors = x_nucleo_ihm02a1->get_components();
//콘솔화면 프린트
printf("Motor Control Application Example for 2 Motors\r\n\n");
//절삭유와 희석수의 현재위치값을 가져온다.
for (int i=2; i<4; i++)
{
EE_ReadVariable(VirtAddVarTab[i], &VarDataTab[i]);
}
//절삭유 현재위치값을 플래쉬 메모리에서 가져온다.
oilcurrposition = VarDataTab[2];
//희석수 현재위치값을 플래쉬 메모리에서 가져온다.
watercurrposition = VarDataTab[3];
//만약 현재의 위치가 홈포지션(0)이 아니라면 홈포지션으로 모터를 회전시킨다.
printf("currunt oil valve position %d",oilcurrposition);
//제일처음 전원이 들어왔을경우 현재값을 초기값으로 지정한다.
printf("--> Set Home position.\r\n");
motors[0]->set_home();
//oilhomeposition = motors[0]->get_position();
oilhomeposition = 0;
motors[1]->set_home();
//waterhomeposition = motors[0]->get_position();
waterhomeposition = 0;
//wait_ms(DELAY_1);
//루프를 돌면서 농도값,밸브열림%,설정,모터제어 관련 작업을 진행한다.
while(1)
{
//현재시간을 체크한다.
NOW = time(NULL);
//현재 농도값을 읽는다.
//아날로그 값은 0~1값으로 들어온다. (참고로 STM32F401보드는 ADC의 분해능이 12bit이다.)
//165옴 저항을 사용했을경우 아래와 같은 범위에서 값이 들어온다.
//4ma = 0.66V, 20ma = 3.3v이다.
//Nucleo-64보드의 아날로그read()함수는 값을 아두이노처럼 0~3.3v사이의 값을 분해능값으로 (즉 0~4095)로
//리턴하는것이 아니라 Voltage값으로 리턴한다.(내부적으로 아날로그값을 Voltage로 바꾸어준다.)
//0~1 사이의 값을 리턴하는데 그값을 보드의 기준전압인 3.3을 곱해주면 현재 아날로그 입력으로 들어오는 전압값을 알수 있다.
//따라서 4ma의 전압값은 0.66v이고 20ma의 값은 3.3v이므로 (입력값-최저값)*100/(최대값-최저값) 계산으로 농도가 몇%인지 계산이 가능하다.
float cc = CurCon.read()* 3.3;
//float cc = CurCon.read()* 3.3;
printf("CC = %f \r\n",cc);
//3.3v신호를 사용할것이므로 165옴의 저항을 연결하면 4ma = 0.66V, 20ma = 3.3v가된다.
//따라서 현재들어오는 전압값을 100분율로 계산하면 몇%의 절삭유농도인지 알수가 있다.
//4ma = -2.0% ~ 20ma = 33.5%
//PV = ((cc - 1)*100.0)/(5.0 - 1);
//0V ~ 3.3V를 백분율로 계산한다.
CPV = ((cc - 0.66)*35.5)/(3.3 - 0.66) - 2.0;
//printf("PV = %f \r\n",PV);
//샘플링 시간을 비교한다.
//샘플링 시간 1초를 지나면 평균값으로 PV값을 입력한다.
if(NOW - SAMPLE >=2)
{
PV = SPV;
samplecnt = 0;
SPV = 0;
SAMPLE = NOW;
}
//샘플링 시간이 지나가지 않았다면 계속 평균값을 구한다.
else
{
if(samplecnt == 0) SPV = CPV;
else
{
SPV = (SPV + CPV) / 2;
}
samplecnt++;
}
//만약 절삭유 농도가 10%(기준농도) 미만이라면 절삭유탱크 밸브를 연다.
if(PV < SV - 0.5)
{
//수탱크 밸브가 열려있다면
if(waterhomeposition != motors[1]->get_position())
{
//수탱크 밸브를 원상복귀 시킨다.
printf("--> Going Home Position Water Tank Value.\r\n");
motors[1]->go_home();
//수탱크 밸브가 HOME 위치로 갈때까지 대기
motors[1]->wait_while_active();
//플래시메모리에 현재의 희석수 밸브 포지션을 저장한다.
VarDataTab[3] = 0;
EE_WriteVariable(VirtAddVarTab[3],VarDataTab[3] );
WATER_VALVE_ON = false;
//현재밸브가 열린%를 계산한다.
WATER_VALVE_OPEN_P = 0.0;
}
//만약 절삭유밸브가 열려있지 않다면 밸브를 연다.
if(oilhomeposition == motors[0]->get_position())
{
//밸브를 앞방향으로 90도(모터2바퀴) 돌린다.
printf("--> Moving forward Oil Tank Valve %d steps.\r\n", STEPS_2);
//모터회전 명령을 내린다.
motors[0]->move(StepperMotor::FWD, STEPS_2);
//모터의 회전이 완료될때까지 기다린다.
motors[0]->wait_while_active();
//절삭유모터의 2바퀴회전후의 포지션을 가져온다.
short oilcurrposition = motors[0]->get_position();
printf("--> Oil Tank Valve Position %d \r\n", oilcurrposition);
//플래시메모리에 현재의 절삭유 밸브 포지션을 저장한다.
VarDataTab[2] = oilcurrposition;
EE_WriteVariable(VirtAddVarTab[2],VarDataTab[2] );
OIL_VALVE_ON = true;
//오일밸브 타이머 시작
//OIL_VALVE_TIMER.start();
OIL_VALVE_OPEN_TIME = time(NULL);
//현재밸브가 열린%를 계산한다.
OIL_VALVE_OPEN_P = (STEPS_2*100)/STEPS_2;
}
//만약 오일밸브 타이머가 시작되어 있다면 시간을 체크한다.
if(OIL_VALVE_ON == true)
{
//만약 절삭유의 현재값이 목표값(설정값) - 0.5 이상이고 절삭유밸브가 OPEN되고 2초이상 지났다면 절삭유가 파이프를 통해서 흘러가는 잔류량을 고려하여 밸브를 닫는다.
if(PV >= SV - 0.5)
{
if((NOW - OIL_VALVE_OPEN_TIME) > 2)
{
printf("--> Going Home Position Oil Tank Value.\r\n");
motors[0]->go_home();
//절삭유탱크 밸브가 HOME 위치로 갈때까지 대기
motors[0]->wait_while_active();
OIL_VALVE_ON = false;
}
}
//printf("--> Aready Oil Tank Valve Opened\r\n");
}
}
//만약 절삭유 농도가 10%(기준농도) < 현재농도 < 10.5% 라면 수탱크 밸브를 연다.
else if(SV + 0.5 <= PV )
{
//절삭유 탱크 밸브가 열려있다면
if(waterhomeposition != motors[0]->get_position())
{
//절삭유 탱크 밸브를 원상복귀 시킨다.
printf("--> Going Home Position Oil Tank Valve\r\n");
motors[0]->go_home();
//절삭유탱크 밸브가 HOME 위치로 갈때까지 대기
motors[0]->wait_while_active();
OIL_VALVE_ON = false;
//현재밸브가 열린%를 계산한다.
OIL_VALVE_OPEN_P = 0.0;
}
//만약 절삭유밸브가 열려있지 않다면 밸브를 연다.
if(waterhomeposition == motors[1]->get_position())
{
//밸브를 앞방향으로 45도(1바퀴) 돌린다.
printf("--> Moving forward Water Tank Valve %d steps.\r\n", STEPS_2);
//모터회전 명령을 내린다.
motors[1]->move(StepperMotor::FWD, STEPS_2);
//모터의 회전이 완료될때까지 기다린다.
motors[1]->wait_while_active();
WATER_VALVE_ON = true;
//희석수탱크밸브 타이머 시작
//WATER_VALVE_TIMER.start()
WATER_VALVE_OPEN_TIME = time(NULL);
//현재밸브가 열린%를 계산한다.
WATER_VALVE_OPEN_P = (STEPS_2*100)/STEPS_2;
}
//만약 수탱크 타이머가 시작되어 있다면 시간을 체크한다.
if(WATER_VALVE_ON == true)
{
//만약 절삭유의 현재값이 기준값(목표값)+ 0.5 이하이고 절삭유밸브가 OPEN되고 2초이상 지났다면 절삭유가 파이프를 통해서 흘러가는 잔류량을 고려하여 밸브를 닫는다.
if(PV <= SV + 0.5)
{
if((NOW - WATER_VALVE_OPEN_TIME) > 2)
{
printf("--> Going Home Position Oil Tank Value.\r\n");
motors[1]->go_home();
//희석수탱크 밸브가 HOME 위치로 갈때까지 대기
motors[1]->wait_while_active();
OIL_VALVE_ON = false;
}
}
}
}
//정상이라면 절삭유탱크밸브 및 수탱크밸브를 HOME위치로 이동시킨다.
else
{
//절삭유 탱크 밸브를 원상복귀 시킨다.
//절삭유 탱크 밸브가 열려있다면
if(waterhomeposition != motors[0]->get_position())
{
printf("--> Going Home Position Oil Tank Value.\r\n");
motors[0]->go_home();
//절삭유탱크 밸브가 HOME 위치로 갈때까지 대기
motors[0]->wait_while_active();
OIL_VALVE_ON = false;
//현재밸브가 열린%를 계산한다.
OIL_VALVE_OPEN_P = 0.0;
}
//희석수탱크 밸브가 열려있다면
if(waterhomeposition != motors[1]->get_position())
{
//수탱크 밸브를 원상복귀 시킨다.
printf("--> Going Home Position Water Tank Value.\r\n");
motors[1]->go_home();
//희석수탱크 밸브가 HOME 위치로 갈때까지 대기
motors[1]->wait_while_active();
WATER_VALVE_ON = false;
//현재밸브가 열린%를 계산한다.
WATER_VALVE_OPEN_P = 0.0;
}
}
//내부적인 프로세스 처리를 우해서 반드시 일정시간 멈추어야한다.
Thread::wait(10);
//wait_ms(100);
}
}