teratail
回答率
85.35%
teratail header banner
teratail header banner
質問するログイン新規登録
トップに関する質問PICがPIDの処理中に落ちます。

編集履歴

質問編集履歴

1

コードの部分を修正しました。ご確認ください

2020/07/02 12:12

投稿

ron4256
ron4256

スコア5

test CHANGED
File without changes
test CHANGED
@@ -46,526 +46,524 @@
46
46
 
47
47
  ```
48
48
 
49
+ #include <xc.h>
50
+
51
+ #include<stdlib.h>
52
+
53
+ #include<math.h>
54
+
55
+
56
+
57
+ // CONFIG1H
58
+
59
+ #pragma config FOSC = INTIO67 // Oscillator Selection bits (Internal oscillator block, port function on RA6 and RA7)
60
+
61
+ #pragma config FCMEN = OFF // Fail-Safe Clock Monitor Enable bit (Fail-Safe Clock Monitor disabled)
62
+
63
+ #pragma config IESO = OFF // Internal/External Oscillator Switchover bit (Oscillator Switchover mode disabled)
64
+
65
+
66
+
67
+ // CONFIG2L
68
+
69
+ #pragma config PWRT = ON // Power-up Timer Enable bit (PWRT disabled)
70
+
71
+ #pragma config BOREN = SBORDIS // Brown-out Reset Enable bits (Brown-out Reset enabled in hardware only (SBOREN is disabled))
72
+
73
+ #pragma config BORV = 18 // Brown Out Reset Voltage bits (VBOR set to 1.8 V nominal)
74
+
75
+
76
+
77
+ // CONFIG2H
78
+
79
+ #pragma config WDTEN = OFF // Watchdog Timer Enable bit (WDT is always enabled. SWDTEN bit has no effect)
80
+
81
+
82
+
83
+ // CONFIG3H
84
+
85
+ #pragma config CCP2MX = PORTC // CCP2 MUX bit (CCP2 input/output is multiplexed with RC1)
86
+
87
+ #pragma config PBADEN = OFF // PORTB A/D Enable bit (PORTB<4:0> pins are configured as analog input channels on Reset)
88
+
89
+ #pragma config LPT1OSC = OFF // Low-Power Timer1 Oscillator Enable bit (Timer1 configured for higher power operation)
90
+
91
+ #pragma config HFOFST = OFF // HFINTOSC Fast Start-up (HFINTOSC starts clocking the CPU without waiting for the oscillator to stablize.)
92
+
93
+ #pragma config MCLRE = OFF // MCLR Pin Enable bit (MCLR pin enabled; RE3 input pin disabled)
94
+
95
+
96
+
97
+ // CONFIG4L
98
+
99
+ #pragma config STVREN = OFF// Stack Full/Underflow Reset Enable bit (Stack full/underflow will cause Reset)
100
+
101
+ #pragma config LVP = OFF // Single-Supply ICSP Enable bit (Single-Supply ICSP enabled)
102
+
103
+ #pragma config XINST = OFF // Extended Instruction Set Enable bit (Instruction set extension and Indexed Addressing mode disabled (Legacy mode))
104
+
105
+
106
+
107
+ // CONFIG5L
108
+
109
+ #pragma config CP0 = OFF // Code Protection Block 0 (Block 0 (000800-001FFFh) not code-protected)
110
+
111
+ #pragma config CP1 = OFF // Code Protection Block 1 (Block 1 (002000-003FFFh) not code-protected)
112
+
113
+
114
+
115
+ // CONFIG5H
116
+
117
+ #pragma config CPB = OFF // Boot Block Code Protection bit (Boot block (000000-0007FFh) not code-protected)
118
+
119
+ #pragma config CPD = OFF // Data EEPROM Code Protection bit (Data EEPROM not code-protected)
120
+
121
+
122
+
123
+ // CONFIG6L
124
+
125
+ #pragma config WRT0 = OFF // Write Protection Block 0 (Block 0 (000800-001FFFh) not write-protected)
126
+
127
+ #pragma config WRT1 = OFF // Write Protection Block 1 (Block 1 (002000-003FFFh) not write-protected)
128
+
129
+
130
+
131
+ // CONFIG6H
132
+
133
+ #pragma config WRTC = OFF // Configuration Register Write Protection bit (Configuration registers (300000-3000FFh) not write-protected)
134
+
135
+ #pragma config WRTB = OFF // Boot Block Write Protection bit (Boot Block (000000-0007FFh) not write-protected)
136
+
137
+ #pragma config WRTD = OFF // Data EEPROM Write Protection bit (Data EEPROM not write-protected)
138
+
139
+
140
+
141
+ // CONFIG7L
142
+
143
+ #pragma config EBTR0 = OFF // Table Read Protection Block 0 (Block 0 (000800-001FFFh) not protected from table reads executed in other blocks)
144
+
145
+ #pragma config EBTR1 = OFF // Table Read Protection Block 1 (Block 1 (002000-003FFFh) not protected from table reads executed in other blocks)
146
+
147
+
148
+
149
+ // CONFIG7H
150
+
151
+ #pragma config EBTRB = OFF // Boot Block Table Read Protection bit (Boot Block (000000-0007FFh) not protected from table reads executed in other blocks)
152
+
153
+
154
+
155
+ // #pragma config statements should precede project file includes.
156
+
157
+ // Use project enums instead of #define for ON and OFF.
158
+
159
+
160
+
161
+
162
+
163
+ #define _XTAL_FREQ 64000000
164
+
165
+
166
+
167
+ int enc_data = 0;
168
+
169
+ unsigned char error_flag = 0;
170
+
171
+
172
+
173
+ int AD_data = 0;
174
+
175
+ //
176
+
177
+
178
+
179
+ void interrupt Data_acquisition(void)
180
+
181
+ {
182
+
183
+
184
+
185
+ RCIE = 0;
186
+
187
+ PEIE = 0;
188
+
189
+ GIE = 0;
190
+
191
+
192
+
193
+ error_flag = 0;
194
+
195
+
196
+
197
+ //serial interrupt
198
+
199
+ if(RCIF == 1)
200
+
201
+ {
202
+
203
+ if(RCSTAbits.FERR || RCSTAbits.OERR){
204
+
205
+ if(RCSTAbits.FERR == 1){
206
+
207
+ error_flag = 0xFE;
208
+
209
+ }
210
+
211
+ else if(RCSTAbits.OERR == 1){
212
+
213
+ error_flag = 0xFF;
214
+
215
+ }
216
+
217
+ RCSTA = 0;
218
+
219
+ RCSTA = 0x90;
220
+
221
+ }
222
+
223
+
224
+
225
+ else{
226
+
227
+ enc_data = RCREG;//encoder data
228
+
229
+ if(enc_data > 1000){
230
+
231
+ enc_data = 0;
232
+
233
+ }
234
+
235
+ }
236
+
237
+ }
238
+
239
+
240
+
241
+ RCIE = 1;
242
+
243
+ PEIE = 1;
244
+
245
+ GIE = 1;
246
+
247
+ }
248
+
249
+
250
+
251
+ void main(void) {
252
+
253
+ int i = 0;
254
+
255
+
256
+
257
+ float P = 4.0;
258
+
259
+ float I = 1.0;
260
+
261
+ float D = 1.0;
262
+
263
+
264
+
265
+ int target = 512;
266
+
267
+
268
+
269
+ //difference
270
+
271
+ float diff1 = 0;
272
+
273
+ float diff2 = 0;
274
+
275
+ float diff3 = 0;
276
+
277
+
278
+
279
+ float PID_calc = 0;
280
+
281
+ int motor_on = 0;
282
+
283
+ int motor_off = 0;
284
+
285
+
286
+
287
+ unsigned char flag_p = 0;
288
+
289
+ unsigned char flag_n = 0;
290
+
291
+ //oscirator setting
292
+
293
+ OSCCON = 0b01110000; //inter osc 64Mhz maybe
294
+
295
+ OSCTUNEbits.PLLEN = 1;
296
+
297
+
298
+
299
+ //PORTA setting
300
+
301
+ ANSEL = 0x01; //PORTA 0pin analog
302
+
303
+ TRISA = 0x01; //RA0 AD convert
304
+
305
+ PORTA = 0;
306
+
307
+ //AD setting
308
+
309
+ ADCON0 = 0x01; //AN0 a/d ADON set
310
+
311
+ ADCON1 = 0x00; //
312
+
313
+ ADCON2 = 0b00111110;; //Fosc/64
314
+
315
+ ADCON2bits.ADFM = 1; //0 left justfited/ 1 right justfited
316
+
317
+
318
+
319
+ //PORTB setting
320
+
321
+ ANSELH = 0x00; //PORTB I/O digital
322
+
323
+ TRISB = 0x00;
324
+
325
+ PORTB = 0;
326
+
327
+
328
+
329
+ //PORTC setting
330
+
331
+ TRISC = 0x80; // RC7 input(rx))
332
+
333
+ PORTC = 0x00;
334
+
335
+
336
+
337
+ //Serial setting
338
+
339
+ TXSTA = 0x24; // TXEN = 1, BRGH = 1
340
+
341
+ RCSTA = 0x90; // SPEN = 1, CREN = 1
342
+
343
+ BAUDCON = 0x08; // 16bitmode
344
+
345
+
346
+
347
+ // 16Bitmode
348
+
349
+ SPBRG = 0x40; // 19.2k
350
+
351
+ SPBRGH = 0x03; // 19.2k
352
+
353
+
354
+
355
+ RCIF = 0;
356
+
357
+
358
+
359
+ RCIE = 1;
360
+
361
+ PEIE = 1;
362
+
363
+ GIE = 1;
364
+
365
+
366
+
367
+ while(1){
368
+
369
+ ADCON0bits.GO = 1;
370
+
371
+ while(ADCON0bits.GO);
372
+
373
+ AD_data = ADRESH * 256 + ADRESL;
374
+
375
+
376
+
377
+ /////////
378
+
379
+
380
+
381
+ if (target > AD_data){
382
+
383
+ flag_p = 1;
384
+
385
+ flag_n = 0;
386
+
387
+ diff1 = target - AD_data;
388
+
389
+ }
390
+
391
+ else if (target < AD_data){
392
+
393
+ flag_p = 0;
394
+
395
+ flag_n = 1;
396
+
397
+ diff1 = AD_data - target;
398
+
399
+ }
400
+
401
+ else if(target == AD_data){
402
+
403
+ flag_p = 1;
404
+
405
+ flag_n = 0;
406
+
407
+ diff1 = 1;
408
+
409
+ }
410
+
411
+
412
+
413
+ PID_calc = P * (diff1 - diff2)
414
+
415
+ + I * diff1
416
+
417
+ + D * (diff1 - 2.0 * diff2 + diff3);
418
+
419
+
420
+
421
+ //float control = P * diff1;
422
+
423
+ // return the distance for this axis:
424
+
425
+ PID_calc = 100 * PID_calc / target;
426
+
427
+ if (PID_calc > 100){
428
+
429
+ PID_calc = 99;
430
+
431
+ }
432
+
433
+
434
+
435
+ if (PID_calc< -100){
436
+
437
+ PID_calc = -99;
438
+
439
+ }
440
+
441
+
442
+
443
+ diff3 = diff2;
444
+
445
+ diff2 = diff1;
446
+
447
+
448
+
449
+ //////
450
+
451
+
452
+
453
+ //duty width on
454
+
455
+ motor_on = abs(PID_calc);
456
+
457
+ //duty width off
458
+
459
+ motor_off = 100 - motor_on;
460
+
461
+
462
+
463
+
464
+
465
+ if (flag_p == 1){
466
+
467
+ RA1 = 1;
468
+
49
- ソースコード
469
+ i = 0;
470
+
471
+ for (i = 0; i < 100; i = i + 1){
472
+
473
+ if (motor_on > 0){
474
+
475
+ //select motor output port
476
+
477
+ RB0 = 1;
478
+
479
+ RB1 = 0;
480
+
481
+ //select delay timer
482
+
483
+ motor_on -= 1;
484
+
485
+ }
486
+
487
+ else if(motor_off > 0){
488
+
489
+ RB0 = 0;
490
+
491
+ RB1 = 0;
492
+
493
+ motor_off -= 1;
494
+
495
+ }
496
+
497
+ }
498
+
499
+ RA1 = 0;
500
+
501
+ }
502
+
503
+
504
+
505
+ else if (flag_n == 1){
506
+
507
+ RA2 = 1;
508
+
509
+ i = 0;
510
+
511
+ for (i = 0; i < 100; i = i + 1){
512
+
513
+ if (motor_on > 0){
514
+
515
+ RB0 = 0;
516
+
517
+ RB1 = 1;
518
+
519
+ motor_on -= 1;
520
+
521
+ }
522
+
523
+ else if(motor_off > 0){
524
+
525
+ RB0 = 0;
526
+
527
+ RB1 = 0;
528
+
529
+ motor_off -= 1;
530
+
531
+ }
532
+
533
+ }
534
+
535
+ RA2 = 0;
536
+
537
+ }
538
+
539
+ //maybe default
540
+
541
+ else if(flag_n == 0 && flag_p == 0){
542
+
543
+ motor_on = 0;
544
+
545
+ motor_off = 0;
546
+
547
+ diff1 = 0;
548
+
549
+ diff2 = 0;
550
+
551
+ diff3 = 0;
552
+
553
+
554
+
555
+ RA3 = 1;
556
+
557
+ }
558
+
559
+ RA3 = 0;
560
+
561
+
562
+
563
+ }
564
+
565
+ return;
566
+
567
+ }
50
568
 
51
569
  ```
52
-
53
- #include <xc.h>
54
-
55
- #include<stdlib.h>
56
-
57
- #include<math.h>
58
-
59
-
60
-
61
- // CONFIG1H
62
-
63
- #pragma config FOSC = INTIO67 // Oscillator Selection bits (Internal oscillator block, port function on RA6 and RA7)
64
-
65
- #pragma config FCMEN = OFF // Fail-Safe Clock Monitor Enable bit (Fail-Safe Clock Monitor disabled)
66
-
67
- #pragma config IESO = OFF // Internal/External Oscillator Switchover bit (Oscillator Switchover mode disabled)
68
-
69
-
70
-
71
- // CONFIG2L
72
-
73
- #pragma config PWRT = ON // Power-up Timer Enable bit (PWRT disabled)
74
-
75
- #pragma config BOREN = SBORDIS // Brown-out Reset Enable bits (Brown-out Reset enabled in hardware only (SBOREN is disabled))
76
-
77
- #pragma config BORV = 18 // Brown Out Reset Voltage bits (VBOR set to 1.8 V nominal)
78
-
79
-
80
-
81
- // CONFIG2H
82
-
83
- #pragma config WDTEN = OFF // Watchdog Timer Enable bit (WDT is always enabled. SWDTEN bit has no effect)
84
-
85
-
86
-
87
- // CONFIG3H
88
-
89
- #pragma config CCP2MX = PORTC // CCP2 MUX bit (CCP2 input/output is multiplexed with RC1)
90
-
91
- #pragma config PBADEN = OFF // PORTB A/D Enable bit (PORTB<4:0> pins are configured as analog input channels on Reset)
92
-
93
- #pragma config LPT1OSC = OFF // Low-Power Timer1 Oscillator Enable bit (Timer1 configured for higher power operation)
94
-
95
- #pragma config HFOFST = OFF // HFINTOSC Fast Start-up (HFINTOSC starts clocking the CPU without waiting for the oscillator to stablize.)
96
-
97
- #pragma config MCLRE = OFF // MCLR Pin Enable bit (MCLR pin enabled; RE3 input pin disabled)
98
-
99
-
100
-
101
- // CONFIG4L
102
-
103
- #pragma config STVREN = OFF// Stack Full/Underflow Reset Enable bit (Stack full/underflow will cause Reset)
104
-
105
- #pragma config LVP = OFF // Single-Supply ICSP Enable bit (Single-Supply ICSP enabled)
106
-
107
- #pragma config XINST = OFF // Extended Instruction Set Enable bit (Instruction set extension and Indexed Addressing mode disabled (Legacy mode))
108
-
109
-
110
-
111
- // CONFIG5L
112
-
113
- #pragma config CP0 = OFF // Code Protection Block 0 (Block 0 (000800-001FFFh) not code-protected)
114
-
115
- #pragma config CP1 = OFF // Code Protection Block 1 (Block 1 (002000-003FFFh) not code-protected)
116
-
117
-
118
-
119
- // CONFIG5H
120
-
121
- #pragma config CPB = OFF // Boot Block Code Protection bit (Boot block (000000-0007FFh) not code-protected)
122
-
123
- #pragma config CPD = OFF // Data EEPROM Code Protection bit (Data EEPROM not code-protected)
124
-
125
-
126
-
127
- // CONFIG6L
128
-
129
- #pragma config WRT0 = OFF // Write Protection Block 0 (Block 0 (000800-001FFFh) not write-protected)
130
-
131
- #pragma config WRT1 = OFF // Write Protection Block 1 (Block 1 (002000-003FFFh) not write-protected)
132
-
133
-
134
-
135
- // CONFIG6H
136
-
137
- #pragma config WRTC = OFF // Configuration Register Write Protection bit (Configuration registers (300000-3000FFh) not write-protected)
138
-
139
- #pragma config WRTB = OFF // Boot Block Write Protection bit (Boot Block (000000-0007FFh) not write-protected)
140
-
141
- #pragma config WRTD = OFF // Data EEPROM Write Protection bit (Data EEPROM not write-protected)
142
-
143
-
144
-
145
- // CONFIG7L
146
-
147
- #pragma config EBTR0 = OFF // Table Read Protection Block 0 (Block 0 (000800-001FFFh) not protected from table reads executed in other blocks)
148
-
149
- #pragma config EBTR1 = OFF // Table Read Protection Block 1 (Block 1 (002000-003FFFh) not protected from table reads executed in other blocks)
150
-
151
-
152
-
153
- // CONFIG7H
154
-
155
- #pragma config EBTRB = OFF // Boot Block Table Read Protection bit (Boot Block (000000-0007FFh) not protected from table reads executed in other blocks)
156
-
157
-
158
-
159
- // #pragma config statements should precede project file includes.
160
-
161
- // Use project enums instead of #define for ON and OFF.
162
-
163
-
164
-
165
-
166
-
167
- #define _XTAL_FREQ 64000000
168
-
169
-
170
-
171
- int enc_data = 0;
172
-
173
- unsigned char error_flag = 0;
174
-
175
-
176
-
177
- int AD_data = 0;
178
-
179
- //
180
-
181
-
182
-
183
- void interrupt Data_acquisition(void)
184
-
185
- {
186
-
187
-
188
-
189
- RCIE = 0;
190
-
191
- PEIE = 0;
192
-
193
- GIE = 0;
194
-
195
-
196
-
197
- error_flag = 0;
198
-
199
-
200
-
201
- //serial interrupt
202
-
203
- if(RCIF == 1)
204
-
205
- {
206
-
207
- if(RCSTAbits.FERR || RCSTAbits.OERR){
208
-
209
- if(RCSTAbits.FERR == 1){
210
-
211
- error_flag = 0xFE;
212
-
213
- }
214
-
215
- else if(RCSTAbits.OERR == 1){
216
-
217
- error_flag = 0xFF;
218
-
219
- }
220
-
221
- RCSTA = 0;
222
-
223
- RCSTA = 0x90;
224
-
225
- }
226
-
227
-
228
-
229
- else{
230
-
231
- enc_data = RCREG;//encoder data
232
-
233
- if(enc_data > 1000){
234
-
235
- enc_data = 0;
236
-
237
- }
238
-
239
- }
240
-
241
- }
242
-
243
-
244
-
245
- RCIE = 1;
246
-
247
- PEIE = 1;
248
-
249
- GIE = 1;
250
-
251
- }
252
-
253
-
254
-
255
- void main(void) {
256
-
257
- int i = 0;
258
-
259
-
260
-
261
- float P = 4.0;
262
-
263
- float I = 1.0;
264
-
265
- float D = 1.0;
266
-
267
-
268
-
269
- int target = 512;
270
-
271
-
272
-
273
- //difference
274
-
275
- float diff1 = 0;
276
-
277
- float diff2 = 0;
278
-
279
- float diff3 = 0;
280
-
281
-
282
-
283
- float PID_calc = 0;
284
-
285
- int motor_on = 0;
286
-
287
- int motor_off = 0;
288
-
289
-
290
-
291
- unsigned char flag_p = 0;
292
-
293
- unsigned char flag_n = 0;
294
-
295
- //oscirator setting
296
-
297
- OSCCON = 0b01110000; //inter osc 64Mhz maybe
298
-
299
- OSCTUNEbits.PLLEN = 1;
300
-
301
-
302
-
303
- //PORTA setting
304
-
305
- ANSEL = 0x01; //PORTA 0pin analog
306
-
307
- TRISA = 0x01; //RA0 AD convert
308
-
309
- PORTA = 0;
310
-
311
- //AD setting
312
-
313
- ADCON0 = 0x01; //AN0 a/d ADON set
314
-
315
- ADCON1 = 0x00; //
316
-
317
- ADCON2 = 0b00111110;; //Fosc/64
318
-
319
- ADCON2bits.ADFM = 1; //0 left justfited/ 1 right justfited
320
-
321
-
322
-
323
- //PORTB setting
324
-
325
- ANSELH = 0x00; //PORTB I/O digital
326
-
327
- TRISB = 0x00;
328
-
329
- PORTB = 0;
330
-
331
-
332
-
333
- //PORTC setting
334
-
335
- TRISC = 0x80; // RC7 input(rx))
336
-
337
- PORTC = 0x00;
338
-
339
-
340
-
341
- //Serial setting
342
-
343
- TXSTA = 0x24; // TXEN = 1, BRGH = 1
344
-
345
- RCSTA = 0x90; // SPEN = 1, CREN = 1
346
-
347
- BAUDCON = 0x08; // 16bitmode
348
-
349
-
350
-
351
- // 16Bitmode
352
-
353
- SPBRG = 0x40; // 19.2k
354
-
355
- SPBRGH = 0x03; // 19.2k
356
-
357
-
358
-
359
- RCIF = 0;
360
-
361
-
362
-
363
- RCIE = 1;
364
-
365
- PEIE = 1;
366
-
367
- GIE = 1;
368
-
369
-
370
-
371
- while(1){
372
-
373
- ADCON0bits.GO = 1;
374
-
375
- while(ADCON0bits.GO);
376
-
377
- AD_data = ADRESH * 256 + ADRESL;
378
-
379
-
380
-
381
- /////////
382
-
383
-
384
-
385
- if (target > AD_data){
386
-
387
- flag_p = 1;
388
-
389
- flag_n = 0;
390
-
391
- diff1 = target - AD_data;
392
-
393
- }
394
-
395
- else if (target < AD_data){
396
-
397
- flag_p = 0;
398
-
399
- flag_n = 1;
400
-
401
- diff1 = AD_data - target;
402
-
403
- }
404
-
405
- else if(target == AD_data){
406
-
407
- flag_p = 1;
408
-
409
- flag_n = 0;
410
-
411
- diff1 = 1;
412
-
413
- }
414
-
415
-
416
-
417
- PID_calc = P * (diff1 - diff2)
418
-
419
- + I * diff1
420
-
421
- + D * (diff1 - 2.0 * diff2 + diff3);
422
-
423
-
424
-
425
- //float control = P * diff1;
426
-
427
- // return the distance for this axis:
428
-
429
- PID_calc = 100 * PID_calc / target;
430
-
431
- if (PID_calc > 100){
432
-
433
- PID_calc = 99;
434
-
435
- }
436
-
437
-
438
-
439
- if (PID_calc< -100){
440
-
441
- PID_calc = -99;
442
-
443
- }
444
-
445
-
446
-
447
- diff3 = diff2;
448
-
449
- diff2 = diff1;
450
-
451
-
452
-
453
- //////
454
-
455
-
456
-
457
- //duty width on
458
-
459
- motor_on = abs(PID_calc);
460
-
461
- //duty width off
462
-
463
- motor_off = 100 - motor_on;
464
-
465
-
466
-
467
-
468
-
469
- if (flag_p == 1){
470
-
471
- RA1 = 1;
472
-
473
- i = 0;
474
-
475
- for (i = 0; i < 100; i = i + 1){
476
-
477
- if (motor_on > 0){
478
-
479
- //select motor output port
480
-
481
- RB0 = 1;
482
-
483
- RB1 = 0;
484
-
485
- //select delay timer
486
-
487
- motor_on -= 1;
488
-
489
- }
490
-
491
- else if(motor_off > 0){
492
-
493
- RB0 = 0;
494
-
495
- RB1 = 0;
496
-
497
- motor_off -= 1;
498
-
499
- }
500
-
501
- }
502
-
503
- RA1 = 0;
504
-
505
- }
506
-
507
-
508
-
509
- else if (flag_n == 1){
510
-
511
- RA2 = 1;
512
-
513
- i = 0;
514
-
515
- for (i = 0; i < 100; i = i + 1){
516
-
517
- if (motor_on > 0){
518
-
519
- RB0 = 0;
520
-
521
- RB1 = 1;
522
-
523
- motor_on -= 1;
524
-
525
- }
526
-
527
- else if(motor_off > 0){
528
-
529
- RB0 = 0;
530
-
531
- RB1 = 0;
532
-
533
- motor_off -= 1;
534
-
535
- }
536
-
537
- }
538
-
539
- RA2 = 0;
540
-
541
- }
542
-
543
- //maybe default
544
-
545
- else if(flag_n == 0 && flag_p == 0){
546
-
547
- motor_on = 0;
548
-
549
- motor_off = 0;
550
-
551
- diff1 = 0;
552
-
553
- diff2 = 0;
554
-
555
- diff3 = 0;
556
-
557
-
558
-
559
- RA3 = 1;
560
-
561
- }
562
-
563
- RA3 = 0;
564
-
565
-
566
-
567
- }
568
-
569
- return;
570
-
571
- }