diff --git a/hoverserial.ino b/hoverserial.ino index ca809c6..5061d72 100644 --- a/hoverserial.ino +++ b/hoverserial.ino @@ -278,8 +278,9 @@ void Nunchuk_control() // ########################## LOOP ########################## -unsigned long iTimeSend = 0; -int idx_led = 0; +unsigned long iTimeSendR = 0, iTimeSendL = 0; +int idx_ledR = 0, idx_ledL = 0; +int speedR, speedL; void loop() { @@ -289,35 +290,77 @@ void loop() digitalWrite(LED_BUILTIN, (timeNow % 2000) < 1000); Receive(); Nunchuk_control(); + + // ********************* simulation ********************************* + // in final project need to use Feedback.speedR_meas and Feedback.speedL_meas + // + speedR = cmd2 + cmd1; + speedL = cmd2 - cmd1; + // + // ********************* simulation ********************************* strip.clear(); - strip.setPixelColor(idx_led, strip.Color(128, 0, 0)); - strip.setPixelColor((NB_STRIP * LED_STRIP) - 1 - idx_led, strip.Color(128, 0, 0)); // second LEDs strip + strip.setPixelColor(idx_ledR, strip.Color(128, 0, 0)); + strip.setPixelColor((NB_STRIP * LED_STRIP) - 1 - idx_ledL, strip.Color(128, 0, 0)); // second LEDs strip strip.show(); - if (iTimeSend > timeNow) - return; - if (cmd2 == 0) - return; - iTimeSend = timeNow + ((200 - abs(cmd2)) * 2); // TIME_SEND - if (cmd2 > 0) + + //*********** 1st led strip ****************** + if (timeNow > iTimeSendR) { - if (idx_led < LED_STRIP - 1) - { - idx_led++; - } - else + if (speedR != 0) { - idx_led = 0; + iTimeSendR = timeNow + ((200 - abs(speedR)) * 2); // TIME_SEND + if (speedR > 0) + { + if (idx_ledR < LED_STRIP - 1) + { + idx_ledR++; + } + else + { + idx_ledR = 0; + } + } + else + { + if (idx_ledR > 0) + { + idx_ledR--; + } + else + { + idx_ledR = LED_STRIP - 1; + } + } } } - else + //*********** 2nd led strip ****************** + if (timeNow > iTimeSendL) { - if (idx_led > 0) - { - idx_led--; - } - else + if (speedL != 0) { - idx_led = LED_STRIP - 1; + iTimeSendL = timeNow + ((200 - abs(speedL)) * 2); // TIME_SEND + if (speedL > 0) + { + if (idx_ledL < LED_STRIP - 1) + { + idx_ledL++; + } + else + { + idx_ledL = 0; + } + } + else + { + if (idx_ledL > 0) + { + idx_ledL--; + } + else + { + idx_ledL = LED_STRIP - 1; + } + } } } }