Jaeyeol Lee / Mbed OS 200117StepMotorControl

200117StepMotorControl

Dependencies:   X_NUCLEO_IHM02A1 TextLCD

main.cpp@28:ed18e436f437, 2020-01-16 (annotated)

Committer:
danjecsr
Date:
Thu Jan 16 06:27:47 2020 +0000
Revision:
28:ed18e436f437
Parent:
27:b25816ce6043
Child:
29:d5c13978c8e9
1st(2020.01.16)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Davidroid 0:5148e9486cf2 1 /**
Davidroid 0:5148e9486cf2 2 ******************************************************************************
danjecsr 28:ed18e436f437 3 이프로그램은 16x2크기의 LCD표시 및 제어, 스텝모터 제어 2가지 기능을 한다.
danjecsr 28:ed18e436f437 4 사용자에서 실시간으로 LCD와 스텝모터의 동시제어를 위하여 2개의 쓰레드로 분리시켰다.
danjecsr 28:ed18e436f437 5 모터가 회전할때는 적어도 1~2초의 시간이 소모되는데, 싱글 쓰레드로 한다면 모터가 작동할때
danjecsr 28:ed18e436f437 6 LCD제어는 멈추게 된다. 따라서 작은시간을 서로 쪼개어 사용하여 서로간의 제어에
danjecsr 28:ed18e436f437 7 영향을 받지 않도록 하기 위하여 2개의 쓰레드로 분리 시켰음.
Davidroid 0:5148e9486cf2 8 ******************************************************************************
Davidroid 0:5148e9486cf2 9 */
Davidroid 0:5148e9486cf2 10
Davidroid 0:5148e9486cf2 11
Davidroid 0:5148e9486cf2 12 /* Includes ------------------------------------------------------------------*/
Davidroid 0:5148e9486cf2 13
danjecsr 28:ed18e436f437 14 // mbed의 기능을 사용하기 위한 헤더파일
Davidroid 0:5148e9486cf2 15 #include "mbed.h"
Davidroid 27:b25816ce6043 16 #include "rtos.h"
Davidroid 0:5148e9486cf2 17
danjecsr 28:ed18e436f437 18
danjecsr 28:ed18e436f437 19 //텍스트LCD를 사용하기 위한 라이브러리의 헤더파일
danjecsr 28:ed18e436f437 20 #include "TextLCD.h"
danjecsr 28:ed18e436f437 21
danjecsr 28:ed18e436f437 22 //내부 FLASH ROM을 사용하기 위한 라이브러리 헤더파일
danjecsr 28:ed18e436f437 23 #include "eeprom.h"
danjecsr 28:ed18e436f437 24 #include <ctype.h>
danjecsr 28:ed18e436f437 25
danjecsr 28:ed18e436f437 26 //스텝모터 드라이버와 SPI통신을 위한 헤더파일
Davidroid 0:5148e9486cf2 27 #include "DevSPI.h"
Davidroid 0:5148e9486cf2 28
danjecsr 28:ed18e436f437 29 //스텝모터 확장보드 사용을 위한 라이브러리 헤더파일
Davidroid 26:caec5f51abe8 30 #include "XNucleoIHM02A1.h"
Davidroid 0:5148e9486cf2 31
Davidroid 0:5148e9486cf2 32
danjecsr 28:ed18e436f437 33
Davidroid 0:5148e9486cf2 34
danjecsr 28:ed18e436f437 35 /*----------------------------------------------LCD제어관련 변수정의 -------------------------------------------------------*/
danjecsr 28:ed18e436f437 36 //16x2 크기의 LCD를 제어하기 위한 객체생성
danjecsr 28:ed18e436f437 37 TextLCD lcd(D10, D9, D8, D7, D6, D5,TextLCD::LCD16x2); // H/L Register(RS) 선택핀, Enable(E) 선택핀, D4,D5,D6,D7(LCD출력핀)
danjecsr 28:ed18e436f437 38 //DigitalOut rw(D7); // H/L Read,Write신호핀
danjecsr 28:ed18e436f437 39
danjecsr 28:ed18e436f437 40 //Digital 입력핀 설정(기본으로 신호가 없을시 풀다운신호(0V)를 출력하도록 설정한다.)
danjecsr 28:ed18e436f437 41 DigitalIn setin(A3,PullDown); //설정버튼
danjecsr 28:ed18e436f437 42 DigitalIn movin(D15,PullDown); //자릿수선택 버튼
danjecsr 28:ed18e436f437 43 DigitalIn upin(D14,PullDown); //값올림 버튼
danjecsr 28:ed18e436f437 44 DigitalIn downin(D2,PullDown); //값내림 버튼
Davidroid 1:9f1974b0960d 45
danjecsr 28:ed18e436f437 46 //DigitalIn set(D9); //설정버튼
danjecsr 28:ed18e436f437 47 //DigitalIn mov(D8); //자릿수선택 버튼
danjecsr 28:ed18e436f437 48 //DigitalIn up(D1); //값올림 버튼
danjecsr 28:ed18e436f437 49 //DigitalIn down(D0); //값내림 버튼
danjecsr 28:ed18e436f437 50 //LCD 출력의 현재시각 측정(단위:초)
danjecsr 28:ed18e436f437 51 time_t NOWLCD;
danjecsr 28:ed18e436f437 52 //LCD 출력을 조절하기 위한 이전시각(NOWLCD)저장
danjecsr 28:ed18e436f437 53 time_t DISPLAY_TIME;
Davidroid 0:5148e9486cf2 54
danjecsr 28:ed18e436f437 55 float SV = 10.5; //절삭유 농도 지시값(기준값)
danjecsr 28:ed18e436f437 56 int SV1 = 1; //화면에 표시될 절삭유 농도 지시값(10의 자릿수값 정수값1문자)
danjecsr 28:ed18e436f437 57 int SV2 = 0; //화면에 표시될 절삭유 농도 지시값(1의 자릿수값 정수값1문자)
danjecsr 28:ed18e436f437 58 int SV3 = 5; //화면에 표시될 절삭유 농도 지시값(소수점1의 자릿수값 정수값1문자)
danjecsr 28:ed18e436f437 59
danjecsr 28:ed18e436f437 60 int SET_SV1 = 0; //설정모드에서 표시될 농도 지시값(10의 자릿수값)
danjecsr 28:ed18e436f437 61 int SET_SV2 = 0; //설정모드에서 표시될 농도 지시값(1의 자릿수값)
danjecsr 28:ed18e436f437 62 int SET_SV3 = 0; //설정모드에서 표시될 농도 지시값(소수점1의 자릿수값)
Davidroid 0:5148e9486cf2 63
Davidroid 0:5148e9486cf2 64
danjecsr 28:ed18e436f437 65 uint16_t VirtAddVarTab[2] = {0x0000, 0x0001};
danjecsr 28:ed18e436f437 66 uint16_t VarDataTab[] = {0, 0};
danjecsr 28:ed18e436f437 67
danjecsr 28:ed18e436f437 68 float PV = 0; //현재측정 절삭유 농도값
danjecsr 28:ed18e436f437 69 float PPV = 0; //이전측정 절각유 농도값
danjecsr 28:ed18e436f437 70
danjecsr 28:ed18e436f437 71 int OIL_VALVE_OPEN_P = 0; //오일밸브 열림%
danjecsr 28:ed18e436f437 72 int WATER_VALVE_OPEN_P = 0; //희석수밸브 열림%
danjecsr 28:ed18e436f437 73 int OIL_VALVE_OPEN_PP = 0; //오일밸브 열림%(이전값)
danjecsr 28:ed18e436f437 74 int WATER_VALVE_OPEN_PP = 0; //희석수밸브 열림%(이전값)
danjecsr 28:ed18e436f437 75
danjecsr 28:ed18e436f437 76 bool set_button = false; //설정버튼 눌림 플래그
danjecsr 28:ed18e436f437 77 bool mov_button = false; //자릿수선택 버튼 눌림 플래그
danjecsr 28:ed18e436f437 78 bool up_button = false; //값올림버튼 눌림 플래그
danjecsr 28:ed18e436f437 79 bool down_button = false; //값내림버튼 눌림 플래그
Davidroid 0:5148e9486cf2 80
danjecsr 28:ed18e436f437 81 bool set_mode = false; //설정버튼 ON/OFF모드
danjecsr 28:ed18e436f437 82 int cursor_loc = 0; //SV값을 설정하기 위하여 현재cursor의 위치를 저장(2번째행 11번열, 12번열 두개값중에 하나를 저장하고 있음)
danjecsr 28:ed18e436f437 83
danjecsr 28:ed18e436f437 84
danjecsr 28:ed18e436f437 85
danjecsr 28:ed18e436f437 86
danjecsr 28:ed18e436f437 87 /*----------------------------------------------STEP모터 제어관련 변수정의 -------------------------------------------------------*/
danjecsr 28:ed18e436f437 88
danjecsr 28:ed18e436f437 89
danjecsr 28:ed18e436f437 90 //스텝수 정의
danjecsr 28:ed18e436f437 91 #define STEPS_1 (200 * 128) //1바퀴에 200스텝(1스텝당 1.8도)을 가지고 있는 스텝모터를 1/128정밀도의 마이크로 스텝을 제어.(1바퀴 회전정의)
danjecsr 28:ed18e436f437 92 #define STEPS_2 (STEPS_1 * 2) //2바퀴 회전정의
danjecsr 28:ed18e436f437 93
danjecsr 28:ed18e436f437 94
danjecsr 28:ed18e436f437 95
danjecsr 28:ed18e436f437 96 //스텝모터 확장모듈 드라이버 객체생성
Davidroid 26:caec5f51abe8 97 XNucleoIHM02A1 *x_nucleo_ihm02a1;
Davidroid 0:5148e9486cf2 98
danjecsr 28:ed18e436f437 99 //스텝모터 파라미터값 정의
davide.aliprandi@st.com 24:d1f487cb02ba 100 L6470_init_t init[L6470DAISYCHAINSIZE] = {
danjecsr 28:ed18e436f437 101 // 첫번째 Motor.(절삭유밸브 모터)
Davidroid 9:f35fbeedb8f4 102 {
danjecsr 28:ed18e436f437 103 12.0, /* Motor supply voltage in V. */
danjecsr 28:ed18e436f437 104 200, /* Min number of steps per revolution for the motor. */
Davidroid 18:591a007effc2 105 1.7, /* Max motor phase voltage in A. */
Davidroid 18:591a007effc2 106 3.06, /* Max motor phase voltage in V. */
Davidroid 18:591a007effc2 107 300.0, /* Motor initial speed [step/s]. */
Davidroid 18:591a007effc2 108 500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
Davidroid 18:591a007effc2 109 500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
Davidroid 18:591a007effc2 110 992.0, /* Motor maximum speed [step/s]. */
Davidroid 18:591a007effc2 111 0.0, /* Motor minimum speed [step/s]. */
Davidroid 18:591a007effc2 112 602.7, /* Motor full-step speed threshold [step/s]. */
Davidroid 18:591a007effc2 113 3.06, /* Holding kval [V]. */
Davidroid 18:591a007effc2 114 3.06, /* Constant speed kval [V]. */
Davidroid 18:591a007effc2 115 3.06, /* Acceleration starting kval [V]. */
Davidroid 18:591a007effc2 116 3.06, /* Deceleration starting kval [V]. */
Davidroid 18:591a007effc2 117 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
Davidroid 18:591a007effc2 118 392.1569e-6, /* Start slope [s/step]. */
Davidroid 18:591a007effc2 119 643.1372e-6, /* Acceleration final slope [s/step]. */
Davidroid 18:591a007effc2 120 643.1372e-6, /* Deceleration final slope [s/step]. */
Davidroid 18:591a007effc2 121 0, /* Thermal compensation factor (range [0, 15]). */
Davidroid 18:591a007effc2 122 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
Davidroid 18:591a007effc2 123 3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
Davidroid 18:591a007effc2 124 StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
Davidroid 18:591a007effc2 125 0xFF, /* Alarm conditions enable. */
Davidroid 18:591a007effc2 126 0x2E88 /* Ic configuration. */
Davidroid 9:f35fbeedb8f4 127 },
Davidroid 9:f35fbeedb8f4 128
danjecsr 28:ed18e436f437 129 //두번째 Motor(희석수밸브 모터)
Davidroid 9:f35fbeedb8f4 130 {
danjecsr 28:ed18e436f437 131 12.0, /* Motor supply voltage in V. */
danjecsr 28:ed18e436f437 132 200, /* Min number of steps per revolution for the motor. */
Davidroid 18:591a007effc2 133 1.7, /* Max motor phase voltage in A. */
Davidroid 18:591a007effc2 134 3.06, /* Max motor phase voltage in V. */
Davidroid 18:591a007effc2 135 300.0, /* Motor initial speed [step/s]. */
Davidroid 18:591a007effc2 136 500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
Davidroid 18:591a007effc2 137 500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
Davidroid 18:591a007effc2 138 992.0, /* Motor maximum speed [step/s]. */
Davidroid 18:591a007effc2 139 0.0, /* Motor minimum speed [step/s]. */
Davidroid 18:591a007effc2 140 602.7, /* Motor full-step speed threshold [step/s]. */
Davidroid 18:591a007effc2 141 3.06, /* Holding kval [V]. */
Davidroid 18:591a007effc2 142 3.06, /* Constant speed kval [V]. */
Davidroid 18:591a007effc2 143 3.06, /* Acceleration starting kval [V]. */
Davidroid 18:591a007effc2 144 3.06, /* Deceleration starting kval [V]. */
Davidroid 18:591a007effc2 145 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
Davidroid 18:591a007effc2 146 392.1569e-6, /* Start slope [s/step]. */
Davidroid 18:591a007effc2 147 643.1372e-6, /* Acceleration final slope [s/step]. */
Davidroid 18:591a007effc2 148 643.1372e-6, /* Deceleration final slope [s/step]. */
Davidroid 18:591a007effc2 149 0, /* Thermal compensation factor (range [0, 15]). */
Davidroid 18:591a007effc2 150 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
Davidroid 18:591a007effc2 151 3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
Davidroid 18:591a007effc2 152 StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
Davidroid 18:591a007effc2 153 0xFF, /* Alarm conditions enable. */
Davidroid 18:591a007effc2 154 0x2E88 /* Ic configuration. */
Davidroid 9:f35fbeedb8f4 155 }
Davidroid 9:f35fbeedb8f4 156 };
Davidroid 9:f35fbeedb8f4 157
Davidroid 0:5148e9486cf2 158
danjecsr 28:ed18e436f437 159
danjecsr 28:ed18e436f437 160 //현재 농도를 받아오는 Analog input 핀번호를 선택한다.(PA_0)즉 핀번호 Analog 0번핀을 선택한다.
danjecsr 28:ed18e436f437 161 AnalogIn CurCon(A0);
danjecsr 28:ed18e436f437 162
danjecsr 28:ed18e436f437 163
danjecsr 28:ed18e436f437 164 //절삭유밸브와 희석수밸브의 초기위치
danjecsr 28:ed18e436f437 165 int oilhomeposition = 0;
danjecsr 28:ed18e436f437 166 int waterhomeposition = 0;
danjecsr 28:ed18e436f437 167
danjecsr 28:ed18e436f437 168 //오일탱크밸브와 수탱크밸브가 오픈된 시간을 저장
danjecsr 28:ed18e436f437 169 Timer OIL_VALVE_TIMER;
danjecsr 28:ed18e436f437 170 Timer WATER_VALVE_TIMER;
danjecsr 28:ed18e436f437 171
danjecsr 28:ed18e436f437 172
danjecsr 28:ed18e436f437 173 //밸브가 열린후 카운트 시간
danjecsr 28:ed18e436f437 174 time_t OIL_VALVE_OPEN_TIME;
danjecsr 28:ed18e436f437 175 time_t WATER_VALVE_OPEN_TIME;
danjecsr 28:ed18e436f437 176
danjecsr 28:ed18e436f437 177 //현재시각
danjecsr 28:ed18e436f437 178 time_t NOW;
danjecsr 28:ed18e436f437 179
danjecsr 28:ed18e436f437 180 //오일탱크밸브와 수탱크밸브가 열려있는지 저장
danjecsr 28:ed18e436f437 181 bool OIL_VALVE_ON = false;
danjecsr 28:ed18e436f437 182 bool WATER_VALVE_ON = false;
danjecsr 28:ed18e436f437 183
danjecsr 28:ed18e436f437 184
danjecsr 28:ed18e436f437 185 //딜레이시간 정의 (milli seconds)
danjecsr 28:ed18e436f437 186 #define DELAY_1 1000
danjecsr 28:ed18e436f437 187 #define DELAY_2 2000
danjecsr 28:ed18e436f437 188 #define DELAY_3 5000
danjecsr 28:ed18e436f437 189
danjecsr 28:ed18e436f437 190
danjecsr 28:ed18e436f437 191
danjecsr 28:ed18e436f437 192 //LCD제어를 위한 쓰레드 생성
danjecsr 28:ed18e436f437 193 void LCD_CTRL_THREAD(void const *args)
danjecsr 28:ed18e436f437 194 {
danjecsr 28:ed18e436f437 195
danjecsr 28:ed18e436f437 196 //설정값을 FLASH메모리에서 읽고쓰기하기 위하여 락을 해제한다.
danjecsr 28:ed18e436f437 197 HAL_FLASH_Unlock();
danjecsr 28:ed18e436f437 198 EE_Init();
danjecsr 28:ed18e436f437 199
danjecsr 28:ed18e436f437 200
danjecsr 28:ed18e436f437 201 //SV값은 총4byte float값인데 EEPROM Library는 기본적으로 2byte씩메모리를 엑세스하게 되어있음
danjecsr 28:ed18e436f437 202 //따라서 2byte메모리를 읽어서 4byte값으로 만들것임.
danjecsr 28:ed18e436f437 203 for (int i=0; i<2; i++)
danjecsr 28:ed18e436f437 204 {
danjecsr 28:ed18e436f437 205 EE_ReadVariable(VirtAddVarTab[i], &VarDataTab[i]);
danjecsr 28:ed18e436f437 206 }
danjecsr 28:ed18e436f437 207
danjecsr 28:ed18e436f437 208 //읽은값을 정수자릿수 2byte + 소숫점1자릿수 2byte값을 합쳐서 4byte float값으로 변경(설정값 저장)
danjecsr 28:ed18e436f437 209 SV = VarDataTab[0] + VarDataTab[1]*0.1;
danjecsr 28:ed18e436f437 210
danjecsr 28:ed18e436f437 211
danjecsr 28:ed18e436f437 212 //SV값을 LCD에 출력하기위하여 SV1,SV2,SV3로 분리한다.
danjecsr 28:ed18e436f437 213 SV1 = (int)(SV/10);
danjecsr 28:ed18e436f437 214 SV2 = (int)(SV-(SV1*10));
danjecsr 28:ed18e436f437 215 SV3 = (int)(((SV-((SV1*10)+SV2))*10)+0.5);
danjecsr 28:ed18e436f437 216
danjecsr 28:ed18e436f437 217
danjecsr 28:ed18e436f437 218
danjecsr 28:ed18e436f437 219
danjecsr 28:ed18e436f437 220 //rw =0;
danjecsr 28:ed18e436f437 221 lcd.cls(); //화면을 초기화 한다.
danjecsr 28:ed18e436f437 222
danjecsr 28:ed18e436f437 223
danjecsr 28:ed18e436f437 224
danjecsr 28:ed18e436f437 225 //절삭유 공급밸브 열림% LCD출력
danjecsr 28:ed18e436f437 226 lcd.locate(0,0);
danjecsr 28:ed18e436f437 227 lcd.printf("P1:");
danjecsr 28:ed18e436f437 228 lcd.printf("%03d%%",OIL_VALVE_OPEN_P);
danjecsr 28:ed18e436f437 229
danjecsr 28:ed18e436f437 230 //절삭유 현재 농도값 LCD출력
danjecsr 28:ed18e436f437 231 lcd.locate(8,0);
danjecsr 28:ed18e436f437 232 lcd.printf("PV:");
danjecsr 28:ed18e436f437 233 lcd.printf("%02.1f%%",PV);
danjecsr 28:ed18e436f437 234
danjecsr 28:ed18e436f437 235 //희석수 공급밸브 열림% LCD출력
danjecsr 28:ed18e436f437 236 lcd.locate(0,1);
danjecsr 28:ed18e436f437 237 lcd.printf("P2:");
danjecsr 28:ed18e436f437 238 lcd.printf("%03d%%",WATER_VALVE_OPEN_P);
danjecsr 28:ed18e436f437 239
danjecsr 28:ed18e436f437 240 //절삭유 지시(목표) 농도값 LCD출력
danjecsr 28:ed18e436f437 241 lcd.locate(8,1);
danjecsr 28:ed18e436f437 242 lcd.printf("SV:");
danjecsr 28:ed18e436f437 243 lcd.printf("%d%d.%d%%",SV1,SV2,SV3);
danjecsr 28:ed18e436f437 244
danjecsr 28:ed18e436f437 245
danjecsr 28:ed18e436f437 246 //커서표시와 깜밖임을 없앤다.
danjecsr 28:ed18e436f437 247 lcd.setCursor(lcd.CurOff_BlkOff);
danjecsr 28:ed18e436f437 248
danjecsr 28:ed18e436f437 249
danjecsr 28:ed18e436f437 250
danjecsr 28:ed18e436f437 251 while (1)
danjecsr 28:ed18e436f437 252 {
danjecsr 28:ed18e436f437 253 NOWLCD = time(NULL);
danjecsr 28:ed18e436f437 254 /*------------------------------- LCD표시 ---------------------------------------------------*/
danjecsr 28:ed18e436f437 255 //1.5초마다 화면을 갱신한다.
danjecsr 28:ed18e436f437 256 if(NOWLCD - DISPLAY_TIME >= 1.5 and !set_mode)
danjecsr 28:ed18e436f437 257 {
danjecsr 28:ed18e436f437 258
danjecsr 28:ed18e436f437 259 //현재 측정한 절삭유 농도값이 이전측정값과 다르다면 LCD화면을 갱신한다.
danjecsr 28:ed18e436f437 260 if(PV != PPV)
danjecsr 28:ed18e436f437 261 {
danjecsr 28:ed18e436f437 262 lcd.locate(11,0);
danjecsr 28:ed18e436f437 263 lcd.printf("%02.1f%%",PV);
danjecsr 28:ed18e436f437 264
danjecsr 28:ed18e436f437 265 //현재값을 이전값변수에 입력한다.(다음타이밍에 비교하기 위하여)
danjecsr 28:ed18e436f437 266 PPV = PV;
danjecsr 28:ed18e436f437 267 }
danjecsr 28:ed18e436f437 268
danjecsr 28:ed18e436f437 269 //현재 오일밸브의 오픈%가 이전값과 달다면 LCD화면을 갱신한다.
danjecsr 28:ed18e436f437 270 if(OIL_VALVE_OPEN_P != OIL_VALVE_OPEN_PP)
danjecsr 28:ed18e436f437 271 {
danjecsr 28:ed18e436f437 272 lcd.locate(3,0);
danjecsr 28:ed18e436f437 273 lcd.printf("%03d%%",OIL_VALVE_OPEN_P);
danjecsr 28:ed18e436f437 274
danjecsr 28:ed18e436f437 275 //현재값을 이전 측정값을 저장하는 변수에 복사
danjecsr 28:ed18e436f437 276 OIL_VALVE_OPEN_PP = OIL_VALVE_OPEN_P;
danjecsr 28:ed18e436f437 277 }
danjecsr 28:ed18e436f437 278
danjecsr 28:ed18e436f437 279
danjecsr 28:ed18e436f437 280 //현재 희석수밸브의 오픈%가 이전값과 달다면 LCD화면을 갱신한다.
danjecsr 28:ed18e436f437 281 if(WATER_VALVE_OPEN_P != WATER_VALVE_OPEN_PP)
danjecsr 28:ed18e436f437 282 {
danjecsr 28:ed18e436f437 283 lcd.locate(3,1);
danjecsr 28:ed18e436f437 284 lcd.printf("%03d%%",WATER_VALVE_OPEN_P);
danjecsr 28:ed18e436f437 285
danjecsr 28:ed18e436f437 286 //현재값을 이전 측정값을 저장하는 변수에 복사
danjecsr 28:ed18e436f437 287 WATER_VALVE_OPEN_PP = WATER_VALVE_OPEN_P;
danjecsr 28:ed18e436f437 288 }
danjecsr 28:ed18e436f437 289
danjecsr 28:ed18e436f437 290 //다음 시간비교를 위해서 현재시각을 저장한다.
danjecsr 28:ed18e436f437 291 DISPLAY_TIME = NOWLCD;
danjecsr 28:ed18e436f437 292 }
danjecsr 28:ed18e436f437 293
danjecsr 28:ed18e436f437 294
danjecsr 28:ed18e436f437 295
danjecsr 28:ed18e436f437 296
danjecsr 28:ed18e436f437 297 /*------------------------------- LCD설정 ---------------------------------------------------*/
danjecsr 28:ed18e436f437 298
danjecsr 28:ed18e436f437 299 //----------농도 설정
danjecsr 28:ed18e436f437 300 //SV(농도지시값)값 설정버튼(SET)을 눌렀을경우 플래그를 세팅한다.(정확한 세팅상태를 지정하기 위함)
danjecsr 28:ed18e436f437 301 if(setin and set_button == false)
danjecsr 28:ed18e436f437 302 {
danjecsr 28:ed18e436f437 303 set_button = true; //설정버튼 플래그 활성화
danjecsr 28:ed18e436f437 304 }
danjecsr 28:ed18e436f437 305
danjecsr 28:ed18e436f437 306 //SET버튼이 눌려지다가 떨어지면 세팅모드로 들어간다.
danjecsr 28:ed18e436f437 307 if(!setin and set_button == true and set_mode == false)
danjecsr 28:ed18e436f437 308 {
danjecsr 28:ed18e436f437 309 printf("LCD SETTING MODE IN\r\n");
danjecsr 28:ed18e436f437 310 lcd.locate(14,1); //설정값 첫번째 위치로 이동
danjecsr 28:ed18e436f437 311 cursor_loc = 14; //현재 커서위치 저장
danjecsr 28:ed18e436f437 312 lcd.setCursor(lcd.CurOff_BlkOn); //설정위치에서 커서 깜박임
danjecsr 28:ed18e436f437 313
danjecsr 28:ed18e436f437 314 set_mode = true; //설정모드 활성화
danjecsr 28:ed18e436f437 315 set_button = false; //설정버튼 플래그 초기화
danjecsr 28:ed18e436f437 316 }
danjecsr 28:ed18e436f437 317
danjecsr 28:ed18e436f437 318 //만약 SV설정 버튼이 true인 상태에서 한번더 누르면 현재값을 저장하고 모드를 빠져나온다(커서의 깜박임을 멈춘다.)
danjecsr 28:ed18e436f437 319 if(!setin and set_button == true and set_mode == true )
danjecsr 28:ed18e436f437 320 {
danjecsr 28:ed18e436f437 321 printf("LCD SETTING MODE OUT\r\n");
danjecsr 28:ed18e436f437 322 SV = (SV1*10) + (SV2*1) + (SV3*0.1); //설정값을 float형(4byte 부동소수점형)으로 저장한다.
danjecsr 28:ed18e436f437 323
danjecsr 28:ed18e436f437 324 //플래쉬메모리에 설정값을 저장한다.
danjecsr 28:ed18e436f437 325
danjecsr 28:ed18e436f437 326 //정수자릿수와 소수점 자릿수로 나눈다.
danjecsr 28:ed18e436f437 327 VarDataTab[0] = (uint16_t)SV; //정수자릿수
danjecsr 28:ed18e436f437 328 VarDataTab[1] = (uint16_t)(((SV-VarDataTab[0])*10)+0.5); //소수점1자릿수(45.5라는 숫자가 45.4999999같은 값이 될수 있으므로 소수점 첫번째에서 반올림한다.)
danjecsr 28:ed18e436f437 329
danjecsr 28:ed18e436f437 330 //내부 플래쉬 메모리에 값을 저장한다.
danjecsr 28:ed18e436f437 331 EE_WriteVariable(VirtAddVarTab[0],VarDataTab[0] );
danjecsr 28:ed18e436f437 332 EE_WriteVariable(VirtAddVarTab[1],VarDataTab[1] );
danjecsr 28:ed18e436f437 333
danjecsr 28:ed18e436f437 334 lcd.setCursor(lcd.CurOff_BlkOff); //설정위치의 커버 깜박임 꺼짐
danjecsr 28:ed18e436f437 335
danjecsr 28:ed18e436f437 336 set_mode = false; //설정모드 종료
danjecsr 28:ed18e436f437 337 set_button = false; //설정버튼 플래그 초기화
danjecsr 28:ed18e436f437 338 }
danjecsr 28:ed18e436f437 339
danjecsr 28:ed18e436f437 340
danjecsr 28:ed18e436f437 341
danjecsr 28:ed18e436f437 342
danjecsr 28:ed18e436f437 343
danjecsr 28:ed18e436f437 344
danjecsr 28:ed18e436f437 345
danjecsr 28:ed18e436f437 346
danjecsr 28:ed18e436f437 347 //-------------자릿수 설정
danjecsr 28:ed18e436f437 348 //자릿수 지정버튼(◁)을 눌렀을경우 플래그를 세팅한다.(정확한 세팅상태를 지정하기 위함)
danjecsr 28:ed18e436f437 349 if(movin and mov_button == false and set_mode == true)
danjecsr 28:ed18e436f437 350 {
danjecsr 28:ed18e436f437 351 mov_button = true; //자릿수 이동버튼 플래그 활성화
danjecsr 28:ed18e436f437 352 }
danjecsr 28:ed18e436f437 353
danjecsr 28:ed18e436f437 354 //만약 현재 설정모드이고 자릿수 버튼이 눌려졌다면 커서를 앞뒤로 이동한다.
danjecsr 28:ed18e436f437 355 if(!movin and mov_button == true and set_mode == true)
danjecsr 28:ed18e436f437 356 {
danjecsr 28:ed18e436f437 357 printf("LCD MOVE\r\n");
danjecsr 28:ed18e436f437 358 //만약 현재 cursor의 위치가 11이라면 14로 옮기고, 12라면 11로, 14라면 12로 옮긴다.
danjecsr 28:ed18e436f437 359 if(cursor_loc == 11)
danjecsr 28:ed18e436f437 360 {
danjecsr 28:ed18e436f437 361 //커서의 위치를 옮긴후 현재의 위치를 저장한다.
danjecsr 28:ed18e436f437 362 lcd.locate(14,1);
danjecsr 28:ed18e436f437 363 cursor_loc = 14;
danjecsr 28:ed18e436f437 364 }
danjecsr 28:ed18e436f437 365 //만약 현재 cursor의 위치가 12라면 11로 옮긴다.
danjecsr 28:ed18e436f437 366 else if(cursor_loc == 12)
danjecsr 28:ed18e436f437 367 {
danjecsr 28:ed18e436f437 368 //커서의 위치를 옮긴후 현재의 위치를 저장한다.
danjecsr 28:ed18e436f437 369 lcd.locate(11,1);
danjecsr 28:ed18e436f437 370 cursor_loc = 11;
danjecsr 28:ed18e436f437 371 }
danjecsr 28:ed18e436f437 372 //만약 현재 cursor의 위치가 14라면 12로 옮긴다.
danjecsr 28:ed18e436f437 373 else if(cursor_loc == 14)
danjecsr 28:ed18e436f437 374 {
danjecsr 28:ed18e436f437 375 //커서의 위치를 옮긴후 현재의 위치를 저장한다.
danjecsr 28:ed18e436f437 376 lcd.locate(12,1);
danjecsr 28:ed18e436f437 377 cursor_loc = 12;
danjecsr 28:ed18e436f437 378 }
danjecsr 28:ed18e436f437 379
danjecsr 28:ed18e436f437 380
danjecsr 28:ed18e436f437 381 mov_button = false; //자릿수 이동버튼 플래그 초기화
danjecsr 28:ed18e436f437 382 }
danjecsr 28:ed18e436f437 383
danjecsr 28:ed18e436f437 384
danjecsr 28:ed18e436f437 385
danjecsr 28:ed18e436f437 386
danjecsr 28:ed18e436f437 387
danjecsr 28:ed18e436f437 388 //------------지시값 설정
danjecsr 28:ed18e436f437 389 //설정모드에서 값올림(△)버튼을 눌렀을 경우 올림버튼 플래그를 활성화 한다.
danjecsr 28:ed18e436f437 390 if(upin and up_button == false and set_mode == true)
danjecsr 28:ed18e436f437 391 {
danjecsr 28:ed18e436f437 392 up_button = true; //값올림 버튼 플래그 활성화
danjecsr 28:ed18e436f437 393 }
danjecsr 28:ed18e436f437 394
danjecsr 28:ed18e436f437 395 //설정모드에서 값올림 버튼을 눌렀다면 현재 커서가 깜박이는 위치의 자릿수 값을 1씩 올린다.
danjecsr 28:ed18e436f437 396 if(!upin and up_button == true and set_mode == true)
danjecsr 28:ed18e436f437 397 {
danjecsr 28:ed18e436f437 398 printf("LCD UP\r\n");
danjecsr 28:ed18e436f437 399 //10의 자릿수에 커서가 위치해 있다면 10의자릿수 값에 1을 더한다.
danjecsr 28:ed18e436f437 400 if(cursor_loc == 11)
danjecsr 28:ed18e436f437 401 {
danjecsr 28:ed18e436f437 402 SV1++;
danjecsr 28:ed18e436f437 403 //만약 더한값이 10이상이라면 다시 0으로 바꾼다.
danjecsr 28:ed18e436f437 404 if(SV1 >= 10) SV1 = 0;
danjecsr 28:ed18e436f437 405 //LCD화면의 값을 바꾼다.
danjecsr 28:ed18e436f437 406 lcd.printf("%d",SV1);
danjecsr 28:ed18e436f437 407 //화면에 프린트하면 location이 자동으로 증가하므로 다시 원래위치로 돌린다.
danjecsr 28:ed18e436f437 408 lcd.locate(11,1);
danjecsr 28:ed18e436f437 409 cursor_loc == 11;
danjecsr 28:ed18e436f437 410
danjecsr 28:ed18e436f437 411 }
danjecsr 28:ed18e436f437 412 //1의 자릿수에 커서가 위치해 있다면 1의자릿수 값에 1을 더한다.
danjecsr 28:ed18e436f437 413 else if(cursor_loc == 12)
danjecsr 28:ed18e436f437 414 {
danjecsr 28:ed18e436f437 415 SV2++;
danjecsr 28:ed18e436f437 416 //만약 더한값이 10이상이라면 다시 0으로 바꾼다.
danjecsr 28:ed18e436f437 417 if(SV2 >= 10) SV2 = 0;
danjecsr 28:ed18e436f437 418 //LCD화면의 값을 바꾼다.
danjecsr 28:ed18e436f437 419 lcd.printf("%d",SV2);
danjecsr 28:ed18e436f437 420 //화면에 프린트하면 location이 자동으로 증가하므로 다시 원래위치로 돌린다.
danjecsr 28:ed18e436f437 421 lcd.locate(12,1);
danjecsr 28:ed18e436f437 422 cursor_loc == 12;
danjecsr 28:ed18e436f437 423 }
danjecsr 28:ed18e436f437 424 //소수점1의 자릿수에 커서가 위치해 있다면 소수점1의자릿수 값에 1을 더한다.
danjecsr 28:ed18e436f437 425 else if(cursor_loc == 14)
danjecsr 28:ed18e436f437 426 {
danjecsr 28:ed18e436f437 427 SV3++;
danjecsr 28:ed18e436f437 428 //만약 더한값이 10이상이라면 다시 0으로 바꾼다.
danjecsr 28:ed18e436f437 429 if(SV3 >= 10) SV3 = 0;
danjecsr 28:ed18e436f437 430 //LCD화면의 값을 바꾼다.
danjecsr 28:ed18e436f437 431 lcd.printf("%d",SV3);
danjecsr 28:ed18e436f437 432 //화면에 프린트하면 location이 자동으로 증가하므로 다시 원래위치로 돌린다.
danjecsr 28:ed18e436f437 433 lcd.locate(14,1);
danjecsr 28:ed18e436f437 434 cursor_loc == 14;
danjecsr 28:ed18e436f437 435 }
danjecsr 28:ed18e436f437 436
danjecsr 28:ed18e436f437 437 up_button = false; //값올림 버튼 플래그 초기화
danjecsr 28:ed18e436f437 438 }
danjecsr 28:ed18e436f437 439
danjecsr 28:ed18e436f437 440
danjecsr 28:ed18e436f437 441
danjecsr 28:ed18e436f437 442
danjecsr 28:ed18e436f437 443 //설정모드에서 값내림(▽)버튼을 눌렀을 경우 내림버튼 플래그를 활성화 한다.
danjecsr 28:ed18e436f437 444 if(downin and down_button == false and set_mode == true)
danjecsr 28:ed18e436f437 445 {
danjecsr 28:ed18e436f437 446 down_button = true; //값내림 버튼 플래그 활성화
danjecsr 28:ed18e436f437 447 }
danjecsr 28:ed18e436f437 448
danjecsr 28:ed18e436f437 449 //설정모드에서 값내림 버튼을 눌렀다면 현재 커서가 깜박이는 위치의 자릿수 값을 1씩 내린다.
danjecsr 28:ed18e436f437 450 if(!downin and down_button == true and set_mode == true)
danjecsr 28:ed18e436f437 451 {
danjecsr 28:ed18e436f437 452 printf("LCD DOWN\r\n");
danjecsr 28:ed18e436f437 453 //10의 자릿수에 커서가 위치해 있다면 10의자릿수 값에 1을 뺀다.
danjecsr 28:ed18e436f437 454 if(cursor_loc == 11)
danjecsr 28:ed18e436f437 455 {
danjecsr 28:ed18e436f437 456 SV1--;
danjecsr 28:ed18e436f437 457 //만약 뺀값이 -1이하라면 값을 9로 바꾼다.
danjecsr 28:ed18e436f437 458 if(SV1 <= -1) SV1 = 9;
danjecsr 28:ed18e436f437 459 //LCD화면의 값을 바꾼다.
danjecsr 28:ed18e436f437 460 lcd.printf("%d",SV1);
danjecsr 28:ed18e436f437 461 //화면에 프린트하면 location이 자동으로 증가하므로 다시 원래위치로 돌린다.
danjecsr 28:ed18e436f437 462 lcd.locate(11,1);
danjecsr 28:ed18e436f437 463 cursor_loc == 11;
danjecsr 28:ed18e436f437 464
danjecsr 28:ed18e436f437 465 }
danjecsr 28:ed18e436f437 466 //1의 자릿수에 커서가 위치해 있다면 1의자릿수 값에 1을 뺀다
danjecsr 28:ed18e436f437 467 else if(cursor_loc == 12)
danjecsr 28:ed18e436f437 468 {
danjecsr 28:ed18e436f437 469 SV2--;
danjecsr 28:ed18e436f437 470 //만약 뺀값이 -1이하라면 값을 9로 바꾼다.
danjecsr 28:ed18e436f437 471 if(SV2 <= -1) SV2 = 9;
danjecsr 28:ed18e436f437 472 //LCD화면의 값을 바꾼다.
danjecsr 28:ed18e436f437 473 lcd.printf("%d",SV2);
danjecsr 28:ed18e436f437 474 //화면에 프린트하면 location이 자동으로 증가하므로 다시 원래위치로 돌린다.
danjecsr 28:ed18e436f437 475 lcd.locate(12,1);
danjecsr 28:ed18e436f437 476 cursor_loc == 12;
danjecsr 28:ed18e436f437 477 }
danjecsr 28:ed18e436f437 478 //소수점1의 자릿수에 커서가 위치해 있다면 소수점1의자릿수 값에 1을 뺀다
danjecsr 28:ed18e436f437 479 else if(cursor_loc == 14)
danjecsr 28:ed18e436f437 480 {
danjecsr 28:ed18e436f437 481 SV3--;
danjecsr 28:ed18e436f437 482 //만약 뺀값이 10이하라면 값을 9로 바꾼다.
danjecsr 28:ed18e436f437 483 if(SV3 <= -1) SV3 = 9;
danjecsr 28:ed18e436f437 484 //LCD화면의 값을 바꾼다.
danjecsr 28:ed18e436f437 485 lcd.printf("%d",SV3);
danjecsr 28:ed18e436f437 486 //화면에 프린트하면 location이 자동으로 증가하므로 다시 원래위치로 돌린다.
danjecsr 28:ed18e436f437 487 lcd.locate(14,1);
danjecsr 28:ed18e436f437 488 cursor_loc == 14;
danjecsr 28:ed18e436f437 489 }
danjecsr 28:ed18e436f437 490
danjecsr 28:ed18e436f437 491 down_button = false; //값내림 버튼 플래그 초기화
danjecsr 28:ed18e436f437 492 }
danjecsr 28:ed18e436f437 493
danjecsr 28:ed18e436f437 494
danjecsr 28:ed18e436f437 495
danjecsr 28:ed18e436f437 496
danjecsr 28:ed18e436f437 497 //쓰레드 대기
danjecsr 28:ed18e436f437 498 Thread::wait(10);
danjecsr 28:ed18e436f437 499 }
danjecsr 28:ed18e436f437 500 }
danjecsr 28:ed18e436f437 501
danjecsr 28:ed18e436f437 502
danjecsr 28:ed18e436f437 503
danjecsr 28:ed18e436f437 504
danjecsr 28:ed18e436f437 505
danjecsr 28:ed18e436f437 506
danjecsr 28:ed18e436f437 507
danjecsr 28:ed18e436f437 508
danjecsr 28:ed18e436f437 509
danjecsr 28:ed18e436f437 510
danjecsr 28:ed18e436f437 511
danjecsr 28:ed18e436f437 512
Davidroid 0:5148e9486cf2 513 /* Main ----------------------------------------------------------------------*/
Davidroid 0:5148e9486cf2 514
Davidroid 0:5148e9486cf2 515 int main()
Davidroid 0:5148e9486cf2 516 {
danjecsr 28:ed18e436f437 517
danjecsr 28:ed18e436f437 518
danjecsr 28:ed18e436f437 519 //LCD제어를 위한 쓰레드를 호출
danjecsr 28:ed18e436f437 520 Thread thread(LCD_CTRL_THREAD);
Davidroid 1:9f1974b0960d 521
danjecsr 28:ed18e436f437 522
danjecsr 28:ed18e436f437 523
danjecsr 28:ed18e436f437 524 //while(1);
danjecsr 28:ed18e436f437 525
danjecsr 28:ed18e436f437 526
danjecsr 28:ed18e436f437 527 //스텝모터 드라이브와 F401RE보드와 통신하기 위하여 SPI통신핀을 초기화 해준다.
Davidroid 23:073b26366d03 528 #ifdef TARGET_STM32F429
Davidroid 23:073b26366d03 529 DevSPI dev_spi(D11, D12, D13);
Davidroid 23:073b26366d03 530 #else
Davidroid 3:fd280b953f77 531 DevSPI dev_spi(D11, D12, D3);
Davidroid 23:073b26366d03 532 #endif
Davidroid 0:5148e9486cf2 533
danjecsr 28:ed18e436f437 534 //확장보드 초기화
Davidroid 26:caec5f51abe8 535 x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
Davidroid 0:5148e9486cf2 536
danjecsr 28:ed18e436f437 537 //모터 컨트롤 모터리스트 객체
davide.aliprandi@st.com 24:d1f487cb02ba 538 L6470 **motors = x_nucleo_ihm02a1->get_components();
Davidroid 0:5148e9486cf2 539
danjecsr 28:ed18e436f437 540 //콘솔화면 프린트
Davidroid 0:5148e9486cf2 541 printf("Motor Control Application Example for 2 Motors\r\n\n");
Davidroid 0:5148e9486cf2 542
Davidroid 1:9f1974b0960d 543
danjecsr 28:ed18e436f437 544 //제일처음 전원이 들어왔을경우 현재값을 초기값으로 지정한다.
danjecsr 28:ed18e436f437 545 printf("--> Set Home position.\r\n");
davide.aliprandi@st.com 24:d1f487cb02ba 546 motors[0]->set_home();
danjecsr 28:ed18e436f437 547 oilhomeposition = motors[0]->get_position();
danjecsr 28:ed18e436f437 548
Davidroid 1:9f1974b0960d 549
danjecsr 28:ed18e436f437 550 motors[1]->set_home();
danjecsr 28:ed18e436f437 551 waterhomeposition = motors[0]->get_position();
danjecsr 28:ed18e436f437 552 //wait_ms(DELAY_1);
danjecsr 28:ed18e436f437 553
danjecsr 28:ed18e436f437 554
danjecsr 28:ed18e436f437 555
Davidroid 1:9f1974b0960d 556
danjecsr 28:ed18e436f437 557 //루프를 돌면서 농도값,밸브열림%,설정,모터제어 관련 작업을 진행한다.
danjecsr 28:ed18e436f437 558 while(1)
danjecsr 28:ed18e436f437 559 {
danjecsr 28:ed18e436f437 560
danjecsr 28:ed18e436f437 561
danjecsr 28:ed18e436f437 562 //현재 농도값을 읽는다.
danjecsr 28:ed18e436f437 563 //아날로그 값은 0~1값으로 들어온다. (참고로 STM32F401보드는 ADC의 분해능이 12bit이다.)
danjecsr 28:ed18e436f437 564 //250옴 저항을 사용했을경우 아래와 같은 범위에서 값이 들어온다.
danjecsr 28:ed18e436f437 565 //4ma = 1V, 20ma = 5v이다.
danjecsr 28:ed18e436f437 566
danjecsr 28:ed18e436f437 567
danjecsr 28:ed18e436f437 568 //Nucleo-64보드의 아날로그read()함수는 값을 아두이노처럼 0~5v사이의 값을 분해능값으로 (즉 0~4095)로
danjecsr 28:ed18e436f437 569 //리턴하는것이 아니라 Voltage값으로 리턴한다.(내부적으로 아날로그값을 Voltage로 바꾸어준다.)
danjecsr 28:ed18e436f437 570 //0~1 사이의 값을 리턴하는데 그값을 보드의 기준전압인 5.0을 곱해주면 현재 아날로그 입력으로 들어오는 전압값을 알수 있다.
danjecsr 28:ed18e436f437 571 //따라서 4ma의 전압값은 1.0v이고 20ma의 값은 5.0v이므로 (입력값-최저값)*100/(최대값-최저값) 계산으로 농도가 몇%인지 계산이 가능하다.
danjecsr 28:ed18e436f437 572
danjecsr 28:ed18e436f437 573 float cc = CurCon.read()* 3.3;
danjecsr 28:ed18e436f437 574 printf("CC = %f \r\n",cc);
danjecsr 28:ed18e436f437 575
danjecsr 28:ed18e436f437 576
danjecsr 28:ed18e436f437 577 //5V시그널을 사용할것이므로 250옴의 저항을 연결하면 4ma = 1.0V, 20ma = 5.0v가된다.
danjecsr 28:ed18e436f437 578 //따라서 현재들어오는 전압값을 100분율로 계산하면 몇%의 절삭유농도인지 알수가 있다.
danjecsr 28:ed18e436f437 579 //PV = ((cc - 1)*100.0)/(5.0 - 1);
danjecsr 28:ed18e436f437 580 //3.3V로 계산
danjecsr 28:ed18e436f437 581 PV = ((cc - 0.66)*100.0)/(3.3 - 0.66);
danjecsr 28:ed18e436f437 582 printf("PV = %f \r\n",PV);
danjecsr 28:ed18e436f437 583
danjecsr 28:ed18e436f437 584
danjecsr 28:ed18e436f437 585
danjecsr 28:ed18e436f437 586
danjecsr 28:ed18e436f437 587
danjecsr 28:ed18e436f437 588
danjecsr 28:ed18e436f437 589 //현재시간을 체크한다.
danjecsr 28:ed18e436f437 590 NOW = time(NULL);
danjecsr 28:ed18e436f437 591
danjecsr 28:ed18e436f437 592
danjecsr 28:ed18e436f437 593 //만약 절삭유 농도가 10%(기준농도) 미만이라면 절삭유탱크 밸브를 연다.
danjecsr 28:ed18e436f437 594 if(PV < SV - 0.5)
danjecsr 28:ed18e436f437 595 {
danjecsr 28:ed18e436f437 596 //수탱크 밸브가 열려있다면
danjecsr 28:ed18e436f437 597 if(waterhomeposition != motors[1]->get_position())
danjecsr 28:ed18e436f437 598 {
danjecsr 28:ed18e436f437 599 //수탱크 밸브를 원상복귀 시킨다.
danjecsr 28:ed18e436f437 600 printf("--> Going Home Position Water Tank Value.\r\n");
danjecsr 28:ed18e436f437 601 motors[1]->go_home();
danjecsr 28:ed18e436f437 602 //수탱크 밸브가 HOME 위치로 갈때까지 대기
danjecsr 28:ed18e436f437 603 motors[1]->wait_while_active();
danjecsr 28:ed18e436f437 604
danjecsr 28:ed18e436f437 605 WATER_VALVE_ON = false;
danjecsr 28:ed18e436f437 606
danjecsr 28:ed18e436f437 607 //현재밸브가 열린%를 계산한다.
danjecsr 28:ed18e436f437 608 WATER_VALVE_OPEN_P = 0.0;
danjecsr 28:ed18e436f437 609 }
danjecsr 28:ed18e436f437 610
danjecsr 28:ed18e436f437 611
danjecsr 28:ed18e436f437 612 //만약 절삭유밸브가 열려있지 않다면 밸브를 연다.
danjecsr 28:ed18e436f437 613 if(oilhomeposition == motors[0]->get_position())
danjecsr 28:ed18e436f437 614 {
danjecsr 28:ed18e436f437 615 //밸브를 앞방향으로 90도(모터2바퀴) 돌린다.
danjecsr 28:ed18e436f437 616 printf("--> Moving forward Oil Tank Valve %d steps.\r\n", STEPS_2);
danjecsr 28:ed18e436f437 617 //모터회전 명령을 내린다.
danjecsr 28:ed18e436f437 618 motors[0]->move(StepperMotor::FWD, STEPS_2);
danjecsr 28:ed18e436f437 619 //모터의 회전이 완료될때까지 기다린다.
danjecsr 28:ed18e436f437 620 motors[0]->wait_while_active();
danjecsr 28:ed18e436f437 621
danjecsr 28:ed18e436f437 622 OIL_VALVE_ON = true;
danjecsr 28:ed18e436f437 623
danjecsr 28:ed18e436f437 624 //오일밸브 타이머 시작
danjecsr 28:ed18e436f437 625 //OIL_VALVE_TIMER.start();
danjecsr 28:ed18e436f437 626 OIL_VALVE_OPEN_TIME = time(NULL);
danjecsr 28:ed18e436f437 627
danjecsr 28:ed18e436f437 628 //현재밸브가 열린%를 계산한다.
danjecsr 28:ed18e436f437 629 OIL_VALVE_OPEN_P = (STEPS_2*100)/STEPS_2;
danjecsr 28:ed18e436f437 630
danjecsr 28:ed18e436f437 631 }
danjecsr 28:ed18e436f437 632
danjecsr 28:ed18e436f437 633 //만약 오일밸브 타이머가 시작되어 있다면 시간을 체크한다.
danjecsr 28:ed18e436f437 634 if(OIL_VALVE_ON == true)
danjecsr 28:ed18e436f437 635 {
danjecsr 28:ed18e436f437 636
danjecsr 28:ed18e436f437 637 //만약 절삭유의 현재값이 목표값(설정값) - 0.5 이상이고 절삭유밸브가 OPEN되고 2초이상 지났다면 절삭유가 파이프를 통해서 흘러가는 잔류량을 고려하여 밸브를 닫는다.
danjecsr 28:ed18e436f437 638 if(PV >= SV - 0.5)
danjecsr 28:ed18e436f437 639 {
danjecsr 28:ed18e436f437 640 if((NOW - OIL_VALVE_OPEN_TIME) > 2)
danjecsr 28:ed18e436f437 641 {
danjecsr 28:ed18e436f437 642 printf("--> Going Home Position Oil Tank Value.\r\n");
danjecsr 28:ed18e436f437 643 motors[0]->go_home();
danjecsr 28:ed18e436f437 644 //절삭유탱크 밸브가 HOME 위치로 갈때까지 대기
danjecsr 28:ed18e436f437 645 motors[0]->wait_while_active();
danjecsr 28:ed18e436f437 646
danjecsr 28:ed18e436f437 647 OIL_VALVE_ON = false;
danjecsr 28:ed18e436f437 648 }
danjecsr 28:ed18e436f437 649 }
danjecsr 28:ed18e436f437 650 //printf("--> Aready Oil Tank Valve Opened\r\n");
danjecsr 28:ed18e436f437 651 }
danjecsr 28:ed18e436f437 652
danjecsr 28:ed18e436f437 653
danjecsr 28:ed18e436f437 654
danjecsr 28:ed18e436f437 655
danjecsr 28:ed18e436f437 656 }
danjecsr 28:ed18e436f437 657 //만약 절삭유 농도가 10%(기준농도) < 현재농도 < 12% 라면 수탱크 밸브를 연다.
danjecsr 28:ed18e436f437 658 else if(SV + 0.5 <= PV )
danjecsr 28:ed18e436f437 659 {
danjecsr 28:ed18e436f437 660 //절삭유 탱크 밸브가 열려있다면
danjecsr 28:ed18e436f437 661 if(waterhomeposition != motors[0]->get_position())
danjecsr 28:ed18e436f437 662 {
danjecsr 28:ed18e436f437 663 //절삭유 탱크 밸브를 원상복귀 시킨다.
danjecsr 28:ed18e436f437 664 printf("--> Going Home Position Oil Tank Valve\r\n");
danjecsr 28:ed18e436f437 665 motors[0]->go_home();
danjecsr 28:ed18e436f437 666 //절삭유탱크 밸브가 HOME 위치로 갈때까지 대기
danjecsr 28:ed18e436f437 667 motors[0]->wait_while_active();
danjecsr 28:ed18e436f437 668
danjecsr 28:ed18e436f437 669 OIL_VALVE_ON = false;
danjecsr 28:ed18e436f437 670
danjecsr 28:ed18e436f437 671 //현재밸브가 열린%를 계산한다.
danjecsr 28:ed18e436f437 672 OIL_VALVE_OPEN_P = 0.0;
danjecsr 28:ed18e436f437 673 }
danjecsr 28:ed18e436f437 674
danjecsr 28:ed18e436f437 675
danjecsr 28:ed18e436f437 676 //만약 절삭유밸브가 열려있지 않다면 밸브를 연다.
danjecsr 28:ed18e436f437 677 if(waterhomeposition == motors[1]->get_position())
danjecsr 28:ed18e436f437 678 {
danjecsr 28:ed18e436f437 679 //밸브를 앞방향으로 45도(1바퀴) 돌린다.
danjecsr 28:ed18e436f437 680 printf("--> Moving forward Water Tank Valve %d steps.\r\n", STEPS_2);
danjecsr 28:ed18e436f437 681 //모터회전 명령을 내린다.
danjecsr 28:ed18e436f437 682 motors[1]->move(StepperMotor::FWD, STEPS_2);
danjecsr 28:ed18e436f437 683 //모터의 회전이 완료될때까지 기다린다.
danjecsr 28:ed18e436f437 684 motors[1]->wait_while_active();
danjecsr 28:ed18e436f437 685
danjecsr 28:ed18e436f437 686 WATER_VALVE_ON = true;
danjecsr 28:ed18e436f437 687
danjecsr 28:ed18e436f437 688 //희석수탱크밸브 타이머 시작
danjecsr 28:ed18e436f437 689 //WATER_VALVE_TIMER.start()
danjecsr 28:ed18e436f437 690 WATER_VALVE_OPEN_TIME = time(NULL);
danjecsr 28:ed18e436f437 691
danjecsr 28:ed18e436f437 692 //현재밸브가 열린%를 계산한다.
danjecsr 28:ed18e436f437 693 WATER_VALVE_OPEN_P = (STEPS_2*100)/STEPS_2;
danjecsr 28:ed18e436f437 694
danjecsr 28:ed18e436f437 695 }
danjecsr 28:ed18e436f437 696
danjecsr 28:ed18e436f437 697 //만약 수탱크 타이머가 시작되어 있다면 시간을 체크한다.
danjecsr 28:ed18e436f437 698 if(WATER_VALVE_ON == true)
danjecsr 28:ed18e436f437 699 {
danjecsr 28:ed18e436f437 700 //만약 절삭유의 현재값이 기준값(목표값)+ 0.5 이하이고 절삭유밸브가 OPEN되고 2초이상 지났다면 절삭유가 파이프를 통해서 흘러가는 잔류량을 고려하여 밸브를 닫는다.
danjecsr 28:ed18e436f437 701 if(PV <= SV + 0.5)
danjecsr 28:ed18e436f437 702 {
danjecsr 28:ed18e436f437 703 if((NOW - WATER_VALVE_OPEN_TIME) > 2)
danjecsr 28:ed18e436f437 704 {
danjecsr 28:ed18e436f437 705 printf("--> Going Home Position Oil Tank Value.\r\n");
danjecsr 28:ed18e436f437 706 motors[1]->go_home();
danjecsr 28:ed18e436f437 707 //희석수탱크 밸브가 HOME 위치로 갈때까지 대기
danjecsr 28:ed18e436f437 708 motors[1]->wait_while_active();
danjecsr 28:ed18e436f437 709
danjecsr 28:ed18e436f437 710 OIL_VALVE_ON = false;
danjecsr 28:ed18e436f437 711 }
danjecsr 28:ed18e436f437 712 }
danjecsr 28:ed18e436f437 713 }
danjecsr 28:ed18e436f437 714
danjecsr 28:ed18e436f437 715 }
danjecsr 28:ed18e436f437 716 //정상이라면 절삭유탱크밸브 및 수탱크밸브를 HOME위치로 이동시킨다.
danjecsr 28:ed18e436f437 717 else
danjecsr 28:ed18e436f437 718 {
danjecsr 28:ed18e436f437 719 //절삭유 탱크 밸브를 원상복귀 시킨다.
danjecsr 28:ed18e436f437 720 //절삭유 탱크 밸브가 열려있다면
danjecsr 28:ed18e436f437 721 if(waterhomeposition != motors[0]->get_position())
danjecsr 28:ed18e436f437 722 {
danjecsr 28:ed18e436f437 723 printf("--> Going Home Position Oil Tank Value.\r\n");
danjecsr 28:ed18e436f437 724 motors[0]->go_home();
danjecsr 28:ed18e436f437 725 //절삭유탱크 밸브가 HOME 위치로 갈때까지 대기
danjecsr 28:ed18e436f437 726 motors[0]->wait_while_active();
danjecsr 28:ed18e436f437 727
danjecsr 28:ed18e436f437 728 OIL_VALVE_ON = false;
danjecsr 28:ed18e436f437 729
danjecsr 28:ed18e436f437 730 //현재밸브가 열린%를 계산한다.
danjecsr 28:ed18e436f437 731 OIL_VALVE_OPEN_P = 0.0;
danjecsr 28:ed18e436f437 732 }
danjecsr 28:ed18e436f437 733
danjecsr 28:ed18e436f437 734
danjecsr 28:ed18e436f437 735 //희석수탱크 밸브가 열려있다면
danjecsr 28:ed18e436f437 736 if(waterhomeposition != motors[1]->get_position())
danjecsr 28:ed18e436f437 737 {
danjecsr 28:ed18e436f437 738 //수탱크 밸브를 원상복귀 시킨다.
danjecsr 28:ed18e436f437 739 printf("--> Going Home Position Water Tank Value.\r\n");
danjecsr 28:ed18e436f437 740 motors[1]->go_home();
danjecsr 28:ed18e436f437 741 //희석수탱크 밸브가 HOME 위치로 갈때까지 대기
danjecsr 28:ed18e436f437 742 motors[1]->wait_while_active();
danjecsr 28:ed18e436f437 743
danjecsr 28:ed18e436f437 744 WATER_VALVE_ON = false;
danjecsr 28:ed18e436f437 745
danjecsr 28:ed18e436f437 746 //현재밸브가 열린%를 계산한다.
danjecsr 28:ed18e436f437 747 WATER_VALVE_OPEN_P = 0.0;
danjecsr 28:ed18e436f437 748 }
danjecsr 28:ed18e436f437 749 }
danjecsr 28:ed18e436f437 750
danjecsr 28:ed18e436f437 751
danjecsr 28:ed18e436f437 752
danjecsr 28:ed18e436f437 753
danjecsr 28:ed18e436f437 754
danjecsr 28:ed18e436f437 755
danjecsr 28:ed18e436f437 756
danjecsr 28:ed18e436f437 757
danjecsr 28:ed18e436f437 758
danjecsr 28:ed18e436f437 759 //내부적인 프로세스 처리를 우해서 반드시 일정시간 멈추어야한다.
danjecsr 28:ed18e436f437 760 Thread::wait(10);
danjecsr 28:ed18e436f437 761
danjecsr 28:ed18e436f437 762 //wait_ms(100);
danjecsr 28:ed18e436f437 763
danjecsr 28:ed18e436f437 764
danjecsr 28:ed18e436f437 765
Davidroid 22:e81ccf73bc5d 766 }
Davidroid 18:591a007effc2 767
Davidroid 1:9f1974b0960d 768
Davidroid 1:9f1974b0960d 769
Davidroid 22:e81ccf73bc5d 770 }