Jaeyeol Lee
200117StepMotorControl
Dependencies: X_NUCLEO_IHM02A1 TextLCD
Diff: main.cpp
- Revision:
- 28:ed18e436f437
- Parent:
- 27:b25816ce6043
- Child:
- 29:d5c13978c8e9
--- a/main.cpp Mon Jul 03 11:53:02 2017 +0000
+++ b/main.cpp Thu Jan 16 06:27:47 2020 +0000
@@ -1,81 +1,107 @@
/**
******************************************************************************
- * @file main.cpp
- * @author Davide Aliprandi, STMicroelectronics
- * @version V1.0.0
- * @date November 4th, 2015
- * @brief mbed test application for the STMicroelectronics X-NUCLEO-IHM02A1
- * Motor Control Expansion Board: control of 2 motors.
- ******************************************************************************
- * @attention
- *
- * <h2><center>© COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- * 3. Neither the name of STMicroelectronics nor the names of its contributors
- * may be used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
- * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
- * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
- * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
+ 이프로그램은 16x2크기의 LCD표시 및 제어, 스텝모터 제어 2가지 기능을 한다.
+ 사용자에서 실시간으로 LCD와 스텝모터의 동시제어를 위하여 2개의 쓰레드로 분리시켰다.
+ 모터가 회전할때는 적어도 1~2초의 시간이 소모되는데, 싱글 쓰레드로 한다면 모터가 작동할때
+ LCD제어는 멈추게 된다. 따라서 작은시간을 서로 쪼개어 사용하여 서로간의 제어에
+ 영향을 받지 않도록 하기 위하여 2개의 쓰레드로 분리 시켰음.
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
-/* mbed specific header files. */
+// mbed의 기능을 사용하기 위한 헤더파일
#include "mbed.h"
#include "rtos.h"
-/* Helper header files. */
+
+//텍스트LCD를 사용하기 위한 라이브러리의 헤더파일
+#include "TextLCD.h"
+
+//내부 FLASH ROM을 사용하기 위한 라이브러리 헤더파일
+#include "eeprom.h"
+#include <ctype.h>
+
+//스텝모터 드라이버와 SPI통신을 위한 헤더파일
#include "DevSPI.h"
-/* Expansion Board specific header files. */
+//스텝모터 확장보드 사용을 위한 라이브러리 헤더파일
#include "XNucleoIHM02A1.h"
-/* Definitions ---------------------------------------------------------------*/
+
-/* Number of movements per revolution. */
-#define MPR_1 4
+/*----------------------------------------------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); //값내림 버튼
-/* Number of steps. */
-#define STEPS_1 (400 * 128) /* 1 revolution given a 400 steps motor configured at 1/128 microstep mode. */
-#define STEPS_2 (STEPS_1 * 2)
+//DigitalIn set(D9); //설정버튼
+//DigitalIn mov(D8); //자릿수선택 버튼
+//DigitalIn up(D1); //값올림 버튼
+//DigitalIn down(D0); //값내림 버튼
+//LCD 출력의 현재시각 측정(단위:초)
+time_t NOWLCD;
+//LCD 출력을 조절하기 위한 이전시각(NOWLCD)저장
+time_t DISPLAY_TIME;
-/* Delay in milliseconds. */
-#define DELAY_1 1000
-#define DELAY_2 2000
-#define DELAY_3 5000
+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의 자릿수값)
-/* Variables -----------------------------------------------------------------*/
+uint16_t VirtAddVarTab[2] = {0x0000, 0x0001};
+uint16_t VarDataTab[] = {0, 0};
+
+float PV = 0; //현재측정 절삭유 농도값
+float PPV = 0; //이전측정 절각유 농도값
+
+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; //값내림버튼 눌림 플래그
-/* Motor Control Expansion Board. */
+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;
-/* Initialization parameters of the motors connected to the expansion board. */
+//스텝모터 파라미터값 정의
L6470_init_t init[L6470DAISYCHAINSIZE] = {
- /* First Motor. */
+ // 첫번째 Motor.(절삭유밸브 모터)
{
- 9.0, /* Motor supply voltage in V. */
- 400, /* Min number of steps per revolution for the 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]. */
@@ -100,10 +126,10 @@
0x2E88 /* Ic configuration. */
},
- /* Second Motor. */
+ //두번째 Motor(희석수밸브 모터)
{
- 9.0, /* Motor supply voltage in V. */
- 400, /* Min number of steps per revolution for the 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]. */
@@ -130,306 +156,615 @@
};
+
+//현재 농도를 받아오는 Analog input 핀번호를 선택한다.(PA_0)즉 핀번호 Analog 0번핀을 선택한다.
+AnalogIn CurCon(A0);
+
+
+//절삭유밸브와 희석수밸브의 초기위치
+int oilhomeposition = 0;
+int waterhomeposition = 0;
+
+//오일탱크밸브와 수탱크밸브가 오픈된 시간을 저장
+Timer OIL_VALVE_TIMER;
+Timer WATER_VALVE_TIMER;
+
+
+//밸브가 열린후 카운트 시간
+time_t OIL_VALVE_OPEN_TIME;
+time_t WATER_VALVE_OPEN_TIME;
+
+//현재시각
+time_t NOW;
+
+//오일탱크밸브와 수탱크밸브가 열려있는지 저장
+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.5 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()
{
- /*----- Initialization. -----*/
+
+
+ //LCD제어를 위한 쓰레드를 호출
+ Thread thread(LCD_CTRL_THREAD);
- /* Initializing SPI bus. */
+
+
+ //while(1);
+
+
+ //스텝모터 드라이브와 F401RE보드와 통신하기 위하여 SPI통신핀을 초기화 해준다.
#ifdef TARGET_STM32F429
DevSPI dev_spi(D11, D12, D13);
#else
DevSPI dev_spi(D11, D12, D3);
#endif
- /* Initializing Motor Control Expansion Board. */
+ //확장보드 초기화
x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
- /* Building a list of motor control components. */
+ //모터 컨트롤 모터리스트 객체
L6470 **motors = x_nucleo_ihm02a1->get_components();
- /* Printing to the console. */
+ //콘솔화면 프린트
printf("Motor Control Application Example for 2 Motors\r\n\n");
- /*----- Setting home and marke positions, getting positions, and going to positions. -----*/
-
- /* Printing to the console. */
- printf("--> Setting home position.\r\n");
-
- /* Setting the home position. */
+ //제일처음 전원이 들어왔을경우 현재값을 초기값으로 지정한다.
+ printf("--> Set Home position.\r\n");
motors[0]->set_home();
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Getting the current position. */
- int position = motors[0]->get_position();
-
- /* Printing to the console. */
- printf("--> Getting the current position: %d\r\n", position);
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Printing to the console. */
- printf("--> Moving forward %d steps.\r\n", STEPS_1);
-
- /* Moving. */
- motors[0]->move(StepperMotor::FWD, STEPS_1);
-
- /* Waiting while active. */
- motors[0]->wait_while_active();
-
- /* Getting the current position. */
- position = motors[0]->get_position();
+ oilhomeposition = motors[0]->get_position();
+
- /* Printing to the console. */
- printf("--> Getting the current position: %d\r\n", position);
-
- /* Printing to the console. */
- printf("--> Marking the current position.\r\n");
-
- /* Marking the current position. */
- motors[0]->set_mark();
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Printing to the console. */
- printf("--> Moving backward %d steps.\r\n", STEPS_2);
-
- /* Moving. */
- motors[0]->move(StepperMotor::BWD, STEPS_2);
-
- /* Waiting while active. */
- motors[0]->wait_while_active();
-
- /* Waiting. */
- wait_ms(DELAY_1);
+ motors[1]->set_home();
+ waterhomeposition = motors[0]->get_position();
+ //wait_ms(DELAY_1);
+
+
+
- /* Getting the current position. */
- position = motors[0]->get_position();
-
- /* Printing to the console. */
- printf("--> Getting the current position: %d\r\n", position);
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Printing to the console. */
- printf("--> Going to marked position.\r\n");
-
- /* Going to marked position. */
- motors[0]->go_mark();
-
- /* Waiting while active. */
- motors[0]->wait_while_active();
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Getting the current position. */
- position = motors[0]->get_position();
-
- /* Printing to the console. */
- printf("--> Getting the current position: %d\r\n", position);
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Printing to the console. */
- printf("--> Going to home position.\r\n");
-
- /* Going to home position. */
- motors[0]->go_home();
-
- /* Waiting while active. */
- motors[0]->wait_while_active();
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Getting the current position. */
- position = motors[0]->get_position();
-
- /* Printing to the console. */
- printf("--> Getting the current position: %d\r\n", position);
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Printing to the console. */
- printf("--> Halving the microsteps.\r\n");
-
- /* Halving the microsteps. */
- init[0].step_sel = (init[0].step_sel > 0 ? init[0].step_sel - 1 : init[0].step_sel);
- if (!motors[0]->set_step_mode((StepperMotor::step_mode_t) init[0].step_sel)) {
- printf(" Step Mode not allowed.\r\n");
+ //루프를 돌면서 농도값,밸브열림%,설정,모터제어 관련 작업을 진행한다.
+ while(1)
+ {
+
+
+ //현재 농도값을 읽는다.
+ //아날로그 값은 0~1값으로 들어온다. (참고로 STM32F401보드는 ADC의 분해능이 12bit이다.)
+ //250옴 저항을 사용했을경우 아래와 같은 범위에서 값이 들어온다.
+ //4ma = 1V, 20ma = 5v이다.
+
+
+ //Nucleo-64보드의 아날로그read()함수는 값을 아두이노처럼 0~5v사이의 값을 분해능값으로 (즉 0~4095)로
+ //리턴하는것이 아니라 Voltage값으로 리턴한다.(내부적으로 아날로그값을 Voltage로 바꾸어준다.)
+ //0~1 사이의 값을 리턴하는데 그값을 보드의 기준전압인 5.0을 곱해주면 현재 아날로그 입력으로 들어오는 전압값을 알수 있다.
+ //따라서 4ma의 전압값은 1.0v이고 20ma의 값은 5.0v이므로 (입력값-최저값)*100/(최대값-최저값) 계산으로 농도가 몇%인지 계산이 가능하다.
+
+ float cc = CurCon.read()* 3.3;
+ printf("CC = %f \r\n",cc);
+
+
+ //5V시그널을 사용할것이므로 250옴의 저항을 연결하면 4ma = 1.0V, 20ma = 5.0v가된다.
+ //따라서 현재들어오는 전압값을 100분율로 계산하면 몇%의 절삭유농도인지 알수가 있다.
+ //PV = ((cc - 1)*100.0)/(5.0 - 1);
+ //3.3V로 계산
+ PV = ((cc - 0.66)*100.0)/(3.3 - 0.66);
+ printf("PV = %f \r\n",PV);
+
+
+
+
+
+
+ //현재시간을 체크한다.
+ NOW = time(NULL);
+
+
+ //만약 절삭유 농도가 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();
+
+ 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();
+
+ 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%(기준농도) < 현재농도 < 12% 라면 수탱크 밸브를 연다.
+ 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);
+
+
+
}
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Printing to the console. */
- printf("--> Setting home position.\r\n");
-
- /* Setting the home position. */
- motors[0]->set_home();
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Getting the current position. */
- position = motors[0]->get_position();
-
- /* Printing to the console. */
- printf("--> Getting the current position: %d\r\n", position);
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Printing to the console. */
- printf("--> Moving forward %d steps.\r\n", STEPS_1);
-
- /* Moving. */
- motors[0]->move(StepperMotor::FWD, STEPS_1);
-
- /* Waiting while active. */
- motors[0]->wait_while_active();
-
- /* Getting the current position. */
- position = motors[0]->get_position();
-
- /* Printing to the console. */
- printf("--> Getting the current position: %d\r\n", position);
-
- /* Printing to the console. */
- printf("--> Marking the current position.\r\n");
-
- /* Marking the current position. */
- motors[0]->set_mark();
-
- /* Waiting. */
- wait_ms(DELAY_2);
-
-
- /*----- Running together for a certain amount of time. -----*/
-
- /* Printing to the console. */
- printf("--> Running together for %d seconds.\r\n", DELAY_3 / 1000);
-
- /* Preparing each motor to perform a run at a specified speed. */
- for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
- motors[m]->prepare_run(StepperMotor::BWD, 400);
- }
-
- /* Performing the action on each motor at the same time. */
- x_nucleo_ihm02a1->perform_prepared_actions();
-
- /* Waiting. */
- wait_ms(DELAY_3);
- /*----- Increasing the speed while running. -----*/
-
- /* Preparing each motor to perform a run at a specified speed. */
- for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
- motors[m]->prepare_get_speed();
- }
-
- /* Performing the action on each motor at the same time. */
- uint32_t* results = x_nucleo_ihm02a1->perform_prepared_actions();
-
- /* Printing to the console. */
- printf(" Speed: M1 %d, M2 %d.\r\n", results[0], results[1]);
-
- /* Printing to the console. */
- printf("--> Doublig the speed while running again for %d seconds.\r\n", DELAY_3 / 1000);
-
- /* Preparing each motor to perform a run at a specified speed. */
- for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
- motors[m]->prepare_run(StepperMotor::BWD, results[m] << 1);
- }
-
- /* Performing the action on each motor at the same time. */
- results = x_nucleo_ihm02a1->perform_prepared_actions();
-
- /* Waiting. */
- wait_ms(DELAY_3);
-
- /* Preparing each motor to perform a run at a specified speed. */
- for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
- motors[m]->prepare_get_speed();
- }
-
- /* Performing the action on each motor at the same time. */
- results = x_nucleo_ihm02a1->perform_prepared_actions();
-
- /* Printing to the console. */
- printf(" Speed: M1 %d, M2 %d.\r\n", results[0], results[1]);
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
-
- /*----- Hard Stop. -----*/
-
- /* Printing to the console. */
- printf("--> Hard Stop.\r\n");
-
- /* Preparing each motor to perform a hard stop. */
- for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
- motors[m]->prepare_hard_stop();
- }
-
- /* Performing the action on each motor at the same time. */
- x_nucleo_ihm02a1->perform_prepared_actions();
-
- /* Waiting. */
- wait_ms(DELAY_2);
-
-
- /*----- Doing a full revolution on each motor, one after the other. -----*/
-
- /* Printing to the console. */
- printf("--> Doing a full revolution on each motor, one after the other.\r\n");
-
- /* Doing a full revolution on each motor, one after the other. */
- for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
- for (int i = 0; i < MPR_1; i++) {
- /* Computing the number of steps. */
- int steps = (int) (((int) init[m].fullstepsperrevolution * pow(2.0f, init[m].step_sel)) / MPR_1);
-
- /* Moving. */
- motors[m]->move(StepperMotor::FWD, steps);
-
- /* Waiting while active. */
- motors[m]->wait_while_active();
-
- /* Waiting. */
- wait_ms(DELAY_1);
- }
- }
-
- /* Waiting. */
- wait_ms(DELAY_2);
-
-
- /*----- High Impedance State. -----*/
-
- /* Printing to the console. */
- printf("--> High Impedance State.\r\n");
-
- /* Preparing each motor to set High Impedance State. */
- for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
- motors[m]->prepare_hard_hiz();
- }
-
- /* Performing the action on each motor at the same time. */
- x_nucleo_ihm02a1->perform_prepared_actions();
-
- /* Waiting. */
- wait_ms(DELAY_2);
}