Xử lý nhiễu này thế nào?

[Hard]

Mã nguồn:
[CODE]
#include <18F4550.h>
#fuses HSPLL,NOWDT,NOPROTECT,NOLVP,NODEBUG,USBDIV,PLL5,CP UDIV1,VREGEN
#use delay(clock=48000000)
#use fast_io(all)
#priority rtcc, rb
#byte FocusStepper = 0x0F81 // Port B
#byte TwoWiresStepper = 0x0F84 // Port E
// Control data for Wave Drive (1 phase on) mode
//BYTE const WaveDrive[4] = {0x01, 0x02, 0x04, 0x08};

// Control data for Full Step Drive (2 phases on) mode
//BYTE const FullStepDrive[4] = {0x03, 0x06, 0x0C, 0x09};

// Control data for Full Step Drive using 2 wire
//BYTE const FullSetpDrive2Wire[4] = {0x01, 0x03, 0x02, 0x00};

// Control data for Half Step Drive (1 & 2 phases on) mode
//BYTE const HalfStepDrive[8] = {0x01, 0x03, 0x02, 0x06, 0x04, 0x0C, 0x08, 0x09};

// Two wires drive
BYTE const TwoWiresDrive[4] = {0x00, 0x01, 0x03, 0x02};
// Control data for Bipolar stepper
BYTE const BipolarDrive[4] = {0x0A, 0x06, 0x05, 0x09};
#define COUNTER_MAX 1000
#define FOCUS_DELAY_TIMEOUT 20
#define TWO_WIRES_DELAY_TIMEOUT 10

volatile int1 FocusRun, FocusDirection;
volatile int1 TwoWiresRun, TwoWiresDirection;

#int_rtcc
void rtcc_isr(void)
{
unsigned int16 Counter;
static unsigned char focus_step, two_wires_step;
static unsigned char focus_delay, two_wires_delay;

set_rtcc(-2500); // reset interval 1ms
if(Counter++ >= COUNTER_MAX)
{
Counter = 0;
output_toggle(PIN_A0);
}
if(FocusRun)
{
if(focus_delay++ >= FOCUS_DELAY_TIMEOUT)
{
focus_delay = 0;
focus_step %= 4;
FocusStepper = BipolarDrive[focus_step];
if(FocusDirection)
focus_step++;
else
focus_step--;
}
}
else
FocusStepper |= 0x0F;
if(TwoWiresRun)
{
if(two_wires_delay++ >= TWO_WIRES_DELAY_TIMEOUT)
{
two_wires_delay = 0;
two_wires_step %= 4;
TwoWiresStepper = TwoWiresDrive[two_wires_step];
if(TwoWiresDirection)
two_wires_step++;
else
two_wires_step--;
}
}
}
#int_rb
void port_b_isr(void)
{
static unsigned char last_b;
unsigned char changes;

changes = last_b ^ input_b();
last_b = input_b();

if (bit_test(changes, 4)&& !bit_test(last_b, 4))
{
//b4 went low
FocusDirection = !FocusDirection;
TwoWiresDirection = !TwoWiresDirection;
}

if (bit_test(changes, 5)&& !bit_test (last_b, 5))
{
//b5 went low
FocusRun = !FocusRun;
TwoWiresRun = !TwoWiresRun;
}
delay_ms (20); //debounce
}
void main(void)
{
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_2);
set_rtcc(-2500); // interval 1ms
enable_interrupts(INT_RTCC);
enable_interrupts(INT_RB);
enable_interrupts(GLOBAL);
set_tris_a(0x00);
set_tris_b(0xF0); // pin 0:3 out, pin 4:7 in
set_tris_c(0x00);
set_tris_d(0x00);
set_tris_e(0x00);
FocusRun = 1;
FocusDirection = 0;
TwoWiresRun = 0;
TwoWiresDirection = 0;
while(1)
{
}
}
[\CODE]