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); } }