Electoday 4.0

ขอคำแนะนำเกี่ยวกับ โค้ด หน่อยครับ

ตอนนี้ผมกำลังจะเขียนโค๊ดจับความเร็วมอเตอร์ เพื่อเอาไปทำเป็นความเร็วของตัวรถแต่ผมเขียนได้แค่จำนวนการตัดผ่านเซ็นเซอร์พี่ๆทุกท่านใครพอรู้ช่วยสอนหรือบอกผมที่ผมจนปัญญาแล้วจริงๆ นี้เป็นอันที่ผมใช้นับอยู่คับint sp1 = 2;
int sp2 = 3;
int count_sp1 = 0;
int count_sp2 = 0;
void setup() {
pinMode(sp1, INPUT);
pinMode(sp2, INPUT);
Serial.begin(9600);
}
void loop() {
if (digitalRead(sp1) == 1) {
count_sp1++;
Serial.print("speed1 = "); Serial.println(count_sp1);
}
if (digitalRead(sp2) == 1) {
count_sp2++;
Serial.print("speed2 = "); Serial.println(count_sp2);
}
}