모터 작동

이제 실제로 모터를 작동하는 코드를 추가해보려 합니다. 모터는 온도가 예열된 상태에서만 작동해야합니다. 버튼을 눌러도 온도가 높지 않다면, 출력이 되지 않게끔 해야 합니다.
따라서 여기에서는 온도에 따라 모터가 작동되도록 코드를 구현하는 것이 목표입니다.

모터 작동 코드를 추가하기전에 현재온도가 목표온도에 근접했는지 확인할 수 있는 코드를 작성해주어야 합니다. 보통 상태를 확인하는 코드는 bool 변수를 사용하면 좋습니다.

※ 전체코드를 계속해서 보여주는 것은 코드가 길기 때문에 작성된 코드에서 추가될 부분만 설명드리겠습니다.

 1// 모터로 재료를 밀어낼 준비가 된 상태인지 확인하는 bool 변수 선언
 2bool isReadyToExt = false;
 3
 4// 현재 상태가 모터를 작동시킬수 있는지 확인하는 함수
 5void checkForReadyToExt()
 6{
 7    if(curTemp > setTemperature - 5)
 8    {
 9        isReadyToExt = true;
10    }
11    else
12    {
13        isReadyToExt = false;
14    }
15}
현재온도가 목표온도에서 5도 차이내에 있다면, 모터를 작동시킬 수 있도록 isReadyToExt 변수를 true로 변경해줍니다.
현재온도가 목표온도보다 높은 경우에는 모터가 작동하는데 문제가 없기 때문에, 온도가 낮은 경우만 고려하였습니다.

하지만 고려해야할 것이 하나 더 있습니다. 어떤 것이 문제가 있을지 한번 생각해보시고 아래 버튼을 눌러 수정된 코드를 확인해보세요.

 1// 모터로 재료를 밀어낼 준비가 된 상태인지 확인하는 bool 변수 선언
 2bool isReadyToExt = false;
 3
 4// 현재 상태가 모터를 작동시킬수 있는지 확인하는 함수
 5void checkForReadyToExt()
 6{
 7    // 설정온도가 0 이라면, isReadyToExt 변수를 false로 저장
 8    if(setTemperature == 0)
 9    {
10        isReadyToExt = false;
11        return;
12    }
13
14    if(curTemp > setTemperature - 5)
15    {
16        isReadyToExt = true;
17    }
18    else
19    {
20        isReadyToExt = false;
21    }
22}
문제는 설정온도가 0 이면, 현재온도보다 항상 낮은 상태가 되니 isReadyToExt 변수가 true 로 저장될 것입니다.
그렇다면 예열이 되지 않은 상태에서도 모터가 움직이게 됩니다.
위 함수에서 저장되는 isReadyToExt 변수를 사용하여 모터 작동코드를 작성하면 아래와 같습니다.
 1// 방향을 알기 위한 true, false를 전처리문으로 선언
 2#define EXT_DIR true
 3#define REV_DIR false
 4
 5/*
 6 * 방향과 속도를 정하여 모터를 작동시키는 함수, 0이면 정지, 255면 최대속도로 통일
 7 */
 8void workMotor(int speed, bool dir)
 9{
10    // 모터를 작동시킬 준비가 되지 않았다면, 함수를 종료
11    if(!isReadyToExt)
12    {
13        return;
14    }
15
16    if(dir==EXT_DIR)
17    {
18        digitalWrite(6, HIGH);
19        analogWrite(10, 255-speed); // HIGH일 경우 스피드를 반전
20    }
21    else if(dir==REV_DIR)
22    {
23        digitalWrite(6, LOW);
24        analogWrite(10,speed);
25    }
26}
모터를 작동하는 함수 앞에 isReadyToExt 의 변수를 확인하는 코드를 추가하여, 온도상태에 따라 모터를 작동시키거나 함수를 종료합니다.

3D 펜에서 재료를 밀어내거나 제거하는 모터의 동작은 다음과 같이 진행됩니다.

../../_images/Step5_1.gif

재료를 밀어내는데에는 밀어내다가 버튼을 손에서 떼면, 살짝 뒤로 당겨줍니다. 이는 필라멘트 재료의 특성 때문에 흘러내림을 방지하기 위함입니다. 이 동작을 리트렉션 이라고 합니다.

아래는 재료를 제거하는 동작입니다.

../../_images/Step5_2.gif

제거하는 동작에는 모터가 오랫동안 회전하여 재료를 전체 다 제거합니다. 또한 이 긴 시간동안 모터만 작동시키게 할 수는 없습니다. 온도도 확인해야하고, 디스플레이에 표시도 해야되기 때문입니다.

그렇기 때문에, 현재 상태파악하고 모터를 작업시킬 함수가 하나 더 필요합니다.

 1// 동작 상태를 나타낼 수 있는 숫자를 표시
 2#define STOP    0 // 모터 정지
 3#define EXTRUDE 1 // 재료 토출
 4#define REVERSE 2 // 재료 제거
 5#define RETRACT 3 // 재료 리트렉션
 6
 7// 동작 상태를 확인할 수 있는 변수 선언
 8int stateAction = 0;
 9
10// 시간을 카운트 할 수 있는 변수 선언
11int timeWork = 0;
12
13/*
14 * 상태 값에 따라 각각의 모터 동작 수행
15 */
16void workAction()
17{
18    switch (stateAction)
19    {
20        case STOP:
21            workMotor(0, EXT_DIR);
22            break;
23
24        case EXTRUDE:
25            workMotor(255, EXT_DIR);
26            break;
27
28        case REVERSE:
29            timeWork--;
30            workMotor(255, REV_DIR);
31            break;
32
33        case RETRACT:
34            timeWork--;
35            workMotor(255,REV_DIR);
36            break;
37    }
38}
setMotorSpeed 는 기존에 선언된 것으로 재료리스트의 속도 값이 저장되는 변수입니다.
동작상태가 여러개이다 보니, bool 변수 대신 int 변수를 사용했습니다. 또한 전처리문으로 코드를 파악하기 쉽도록 하였습니다.

timeWork 변수는 시간을 측정하는데 사용합니다. 예를 들어 아래와 같은 코드가 있다면, timeWork이 0이 되기 까지 1초가 걸립니다.
 1int timeWork = 20;
 2long time = millis();
 3
 4void setup()
 5{
 6
 7}
 8
 9void loop()
10{
11    if(millis() - time > 50)
12    {
13        timeWork--;
14    }
15}
이와 같은 원리로 timeWork의 변수를 선언 하였습니다.

기존 코드에서 workMotor, workAction, isReadyToExt 함수를 추가하여 코드를 작성해봅니다.
1. A 버튼을 누른 상태면, 모터가 필라멘트 토출되는 방향으로 회전.
2. A 버튼을 누른 상태에서 떼면, 모터가 필라멘트 제거되는 방향으로 1초 동안 회전.
3. B 버튼을 누르면, 모터가 필라멘트 제거되는 방향으로 10초동안 회전.
4. A,B 버튼을 누르지 않으면, 모터는 정지 하도록 합니다.

작성후 아래의 코드와 비교해봅니다.
  1#include "ssd1306.h" // 라이브러리 포함
  2
  3#define BTN_A           8      // A버튼
  4#define BTN_B           7      // B버튼
  5#define BTN_C           11     // C버튼
  6#define BTN_D           12     // D버튼
  7
  8#define MOTOR_EN        5      // 모터 활성화 핀
  9#define MOTOR_DIR       6      // 모터 방향 핀
 10#define MOTOR_SPEED     10     // 모터 속도 핀
 11
 12#define HEATER_EN       9      // 열선 핀
 13
 14#define TEMP_IN         A0     // 온도 읽는 핀
 15
 16#define VALUE_TEMPTB    0      // 온도 테이블의 신호 값 인덱스
 17#define CELSIUS_TEMPTB  1      // 온도 테이블의 온도 값 인덱스
 18
 19#define MATERIAL_COUNT  3      // material 구조체 배열의 갯수
 20
 21// 동작 상태를 나타낼 수 있는 숫자를 표시
 22#define STOP            0 // 모터 정지
 23#define EXTRUDE         1 // 재료 토출
 24#define REVERSE         2 // 재료 제거
 25#define RETRACT         3 // 재료 리트렉션
 26
 27// 모터 방향을 알기 위한 true, false를 전처리문으로 선언
 28#define EXT_DIR         true
 29#define REV_DIR         false
 30
 31String strToShow;              // 디스플레이에 보여줄 문자열을 저장하는 변수
 32
 33bool isHeating = false; // 온도가 목표보다 높은지 확인하는 bool 변수
 34bool isNeedUpdateScreen = true;      // 업데이트가 필요한지 확인하는 bool 변수 생성
 35bool isPressedA_BTN, isPressedB_BTN, isPressedC_BTN, isPressedD_BTN; // 버튼이 눌러져 있는지 확인하는 bool 변수 생성
 36bool isReadyToExt = false; // 모터로 재료를 밀어낼 준비가 된 상태인지 확인하는 bool 변수 선언
 37
 38int stateAction = 0; // 동작 상태를 확인할 수 있는 변수 선언
 39int timeWork = 0; // 시간을 카운트 할 수 있는 변수 선언
 40int curTemp = 0; // 현재온도
 41
 42long timeInterval  = millis(); // 50ms 에 1회씩 작동하기 위한 시간 변수
 43
 44int material_index = 0;        // 재료 구조체 배열의 위치를 가리키는 인덱스
 45int setTemperature = 0;        // 설정된 온도
 46
 47// 온도, 속도, 재료명을 포함한 구조체 선언
 48struct setting {
 49    int temperature;
 50    char* materialName;
 51};
 52
 53struct setting materials[] = {{0, "OFF"}, {60, "PCL"}, {200, "PLA"}};
 54
 55/*
 56 *  온도 테이블 배열
 57 *  첫번째 항목은 신호 값, 두번째 항목은 온도 값
 58 */
 59int temptable[23][2] = {
 60    {1023,0},
 61    {1022,10},
 62    {1020,20},
 63    {1016,30},
 64    {1011,40},
 65    {1009,50},
 66    {1006,60},
 67    {1004,70},
 68    {1000,80},
 69    {990,90},
 70    {983,100},
 71    {976,110},
 72    {972,120},
 73    {964,130},
 74    {955,140},
 75    {942,150},
 76    {929,160},
 77    {910,170},
 78    {895,180},
 79    {864,190},
 80    {839,200},
 81    {800,210},
 82    {744,220}
 83};
 84
 85/*
 86 * 입력받은 문자를 디스플레이 좌표에 표시
 87 */
 88void showTextToScreen(int x, int y, String text)
 89{
 90    text = text + "\n";
 91    char ch[10];
 92    text.toCharArray(ch,text.length());
 93    ssd1306_printFixedN(x, y, ch, STYLE_NORMAL, FONT_SIZE_2X);
 94}
 95
 96/*
 97 * 현재 상태가 모터를 작동시킬수 있는지 확인하는 함수
 98 */
 99void checkForReadyToExt()
100{
101    // 설정온도가 0 이라면, isReadyToExt 변수를 false로 저장
102    if(setTemperature == 0)
103    {
104        isReadyToExt = false;
105        return;
106    }
107
108    if(curTemp > setTemperature - 5)
109    {
110        isReadyToExt = true;
111    }
112    else
113    {
114        isReadyToExt = false;
115    }
116}
117
118/*
119 * 방향과 속도를 정하여 모터를 작동시키는 함수, 0이면 정지, 255면 최대속도로 통일
120 */
121void workMotor(int speed, bool dir)
122{
123    // 모터를 작동시킬 준비가 되지 않았다면, 함수를 종료
124    if(!isReadyToExt)
125    {
126        return;
127    }
128
129    if(dir==EXT_DIR)
130    {
131        digitalWrite(6, HIGH);
132        analogWrite(10, 255-speed); // HIGH일 경우 스피드를 반전
133    }
134    else if(dir==REV_DIR)
135    {
136        digitalWrite(6, LOW);
137        analogWrite(10,speed);
138    }
139}
140
141/*
142 * 상태 값에 따라 각각의 모터 동작 수행
143 */
144void workAction()
145{
146    switch (stateAction)
147    {
148        case STOP:
149            workMotor(0, EXT_DIR);
150            break;
151
152        case EXTRUDE:
153            workMotor(255, EXT_DIR);
154            break;
155
156        case REVERSE:
157            timeWork--;
158            workMotor(255, REV_DIR);
159            if(timeWork == 0)
160            {
161                stateAction = STOP;
162            }
163            break;
164
165        case RETRACT:
166            timeWork--;
167            workMotor(255,REV_DIR);
168            if(timeWork == 0)
169            {
170                stateAction = STOP;
171            }
172            break;
173    }
174}
175
176/*
177 * 온도를 읽고, 정확한 온도로 계산 후 결과 값을 화면에 표시하고 반환하는 함수
178 * VALUE_TEMPTB = 0, CELSIUS_TEMPTB = 1 으로 온도표의 각 항목을 지시함
179 */
180int getTemperature()
181{
182    float ratioTemp;
183    float tempADU = analogRead(A0);
184    int result;
185
186    for(int i=1; i<23; i++){
187        if(tempADU >= temptable[i][VALUE_TEMPTB])
188        {
189            ratioTemp = (tempADU - temptable[i][VALUE_TEMPTB])/(temptable[i-1][VALUE_TEMPTB] - temptable[i][VALUE_TEMPTB]);
190
191            result = temptable[i][CELSIUS_TEMPTB] - ratioTemp*(temptable[i][CELSIUS_TEMPTB] - temptable[i-1][CELSIUS_TEMPTB]);
192
193            if(result < 10)
194            {
195                strToShow = "00" + String(result);
196            }
197            else if(result < 100)
198            {
199                strToShow = "0" + String(result);
200            }
201            else
202            {
203                strToShow = String(result);
204            }
205
206            showTextToScreen(0,16,strToShow);
207
208            return result;
209        }
210    }
211
212    return ;
213}
214
215float Kp=36, Ki=2.5, Kd=0.625, dT = 0.05;
216int16_t error, previousError = 0; // 오차 변수
217float integral, derivaitve = 0;   // 적분 미분 변수
218
219/*
220* pid 계산을 하고, 결과값을 HEATER_EN 핀에 적용
221*/
222void getPIDoutput(int targetTemp, int actualValue)
223{
224    float outputValue;
225
226    //설정온도에 도달하기 10도 전일때부터 PID 제어 시작
227     if(curTemp < setTemperature - 10)
228    {
229        outputValue = 255;
230    }
231    else
232    {
233        // 에러 값 저장
234        error      = targetTemp - actualValue;
235
236        // 적분 값 저장
237        integral   = integral + (float)error*dT;
238
239        // 미분 값 저장
240        derivaitve = ((float)error - (float)previousError)/dT;
241        previousError = error;
242
243        // PID 계산
244        outputValue = Kp*error + Ki*integral + Kd*derivaitve;
245
246        // output 값이 0~255 범위를 벗어나면 최대, 최소 값을 대신 저장
247        if(outputValue > 255)
248        {
249            outputValue = 255;
250        }
251        else if(outputValue <0)
252        {
253            outputValue = 0;
254        }
255
256        // outputValue 가 0 혹은 OFF 라면 예열이 되지 않음으로, isHeating 변수를 false로 저장
257        if(outputValue == 0 || material_index == 0)
258        {
259            isHeating = false;
260        }
261        else
262        {
263            isHeating = true;
264        }
265    }
266    // 계산된 결과값을 디지털 9번핀에 analogWrite 로 입력
267    analogWrite(HEATER_EN,outputValue);
268}
269
270/*
271 * 버튼이 눌러졌는지 확인하고, 해당버튼에 맞는 코드 실행
272 */
273void checkBtnPressed()
274{
275    // A버튼이 눌리고, B버튼이 눌리지 않았을 경우 실행
276    if(!digitalRead(BTN_A) && digitalRead(BTN_B))
277    {
278        // A 버튼이 눌러져 있으면(isPressedA_BTN 가 true), 아래 코드 건너뜀
279        if(!isPressedA_BTN)
280        {
281            stateAction = EXTRUDE;
282        }
283    }
284    // B버튼이 눌리고, A버튼이 눌리지 않았을 경우 실행
285    else if(digitalRead(BTN_A) && !digitalRead(BTN_B))
286    {
287        // B 버튼이 눌러져 있으면(isPressedB_BTN 가 true), 아래 코드 건너뜀
288        if(!isPressedB_BTN)
289        {
290            // 동작 상태를 필라멘트 제거로 변경하고,
291            // 시간을 10초 동안 작동하도록 timeWork 변수에 값 저장
292            stateAction = REVERSE;
293            timeWork = 200;
294        }
295    }
296    else
297    {
298        // A, B 버튼 모두 눌리지 않을 경우, 변수 값 변경
299        isPressedA_BTN = false;
300        isPressedB_BTN = false;
301
302        // 동작 상태를 리렉션으로 변경하고,
303        // 시간을 1초 동안 작동하도록 timeWork 변수에 값 저장
304        if(stateAction == EXTRUDE)
305        {
306            stateAction = RETRACT;
307            timeWork = 20;
308        }
309    }
310
311
312    // C버튼이 눌리고, D버튼이 눌리지 않았을 경우 실행
313    if(!digitalRead(BTN_C) && digitalRead(BTN_D))
314    {
315        // C 버튼이 눌러져 있으면(isPressedC_BTN 가 true), 아래 코드 건너뜀
316        if(!isPressedC_BTN)
317        {
318            // 구조체 배열의 인덱스로 사용될 변수 값 1 증가
319            material_index++;
320            if(material_index > MATERIAL_COUNT-1)
321            {
322                material_index = 0;
323            }
324
325            // 디스플레이 업데이트를 할 수 있도록 변수 변경
326            isNeedUpdateScreen = true;
327
328            // 버튼 상태 변수 변경
329            isPressedC_BTN = true;
330
331            // 적분 값 리셋
332            integral = 0;
333        }
334    }
335    // D버튼이 눌리고, C버튼이 눌리지 않았을 경우 실행
336    else if(digitalRead(BTN_C) && !digitalRead(BTN_D))
337    {
338        // D 버튼이 눌러져 있으면(isPressedD_BTN 가 true), 아래 코드 건너뜀
339        if(!isPressedD_BTN)
340        {
341            // 구조체 배열의 인덱스로 사용될 변수 값 1 감소
342            material_index--;
343            if(material_index < 0)
344            {
345                material_index = MATERIAL_COUNT-1;
346            }
347
348            // 디스플레이 업데이트를 할 수 있도록 변수 변경
349            isNeedUpdateScreen = true;
350
351            // 버튼 상태 변수 변경
352            isPressedD_BTN = true;
353
354            // 적분 값 리셋
355            integral = 0;
356        }
357    }
358    else
359    {
360        // C, D 버튼 모두 눌리지 않을 경우, 변수 값 변경
361        isPressedC_BTN = false;
362        isPressedD_BTN = false;
363    }
364
365}
366
367/*
368 * 화면에 표시될 재료명, 목표속도, 목표온도를 업데이트
369 */
370void updateMaterial(int index)
371{
372    // isNeedUpdate 변수가 false 이면 함수를 종료함
373    if(!isNeedUpdateScreen) return;
374
375    // 목표 온도, 속도 항목에 현재 인덱스의 구조체 값을 저장
376    setTemperature = materials[index].temperature;
377
378    // 화면 클리어
379    ssd1306_clearScreen();
380
381    // 화면에 목표 온도, 재료 명, 속도를 표시
382    // 재료명은 좌표 0,0 에 표시
383    // 목표 속도는 좌표 80, 0 에 표시
384    // 목표 온도는 좌표 52, 16 에 표시
385    strToShow = materials[index].materialName;
386    showTextToScreen(0,0, strToShow);
387
388    strToShow = "/" + String(setTemperature);
389    showTextToScreen(52,16,strToShow);
390
391    // 화면을 계속해서 업데이트 하지 않도록 방지하는 bool 변수 변경
392    isNeedUpdateScreen = false;
393}
394
395void setup()
396{
397    pinMode(BTN_A, INPUT_PULLUP);
398    pinMode(BTN_B, INPUT_PULLUP);
399    pinMode(BTN_C, INPUT_PULLUP);
400    pinMode(BTN_D, INPUT_PULLUP);
401
402    pinMode(MOTOR_EN, OUTPUT);
403    pinMode(MOTOR_DIR, OUTPUT);
404    pinMode(MOTOR_SPEED, OUTPUT);
405
406    pinMode(HEATER_EN, OUTPUT);
407
408    digitalWrite(MOTOR_EN, HIGH); // 모터 활성화
409
410    ssd1306_128x32_i2c_init(); // 32로 변경
411    ssd1306_fillScreen(0x00);  // 화면 초기화
412    ssd1306_setFixedFont(ssd1306xled_font6x8); // 폰트 설정
413    ssd1306_flipHorizontal(1); // x 화면 대칭 회전
414    ssd1306_flipVertical(1);   // y 화면 대칭 회전
415}
416
417void loop()
418{
419    if(millis() - timeInterval > 50)
420    {
421        curTemp = getTemperature();
422        updateMaterial(material_index);
423        checkBtnPressed();
424        getPIDoutput(setTemperature, curTemp);
425        checkForReadyToExt();
426        workAction();
427
428        timeInterval = millis();
429    }
430}