Created
February 28, 2017 05:04
-
-
Save thorikawa/717b73007ea38f02d2fff7ec05b7a24f to your computer and use it in GitHub Desktop.
Arduino sketch for nanabo
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include <VarSpeedServoSam.h> | |
VarSpeedServoSam servos[4]; | |
int MAX_SPEED = 50; | |
int PIN_NO[] = {2, 3, 4, 5}; | |
int MAX_DEGREE[] = {110, 110, 120, 100}; | |
int MIN_DEGREE[] = {70, 90, 80, 80}; | |
int INITIAL_DEGREE[] = {90, 90, 90, 90}; | |
int flag, sid, degree, speed; | |
void setup() { | |
Serial.begin(57600); | |
while (!Serial) { | |
} | |
// initial position | |
for (int i = 0; i<4; i++) { | |
servos[i].attach(PIN_NO[i]); | |
servos[i].write(INITIAL_DEGREE[i]); | |
} | |
} | |
void loop() { | |
int n = Serial.available(); | |
while (n > 0) { | |
// Serial.println(n); | |
int flag = Serial.read(); | |
n--; | |
if (flag == 0xFE) { | |
if (n < 3) { | |
break; | |
} | |
sid = Serial.read(); | |
degree = Serial.read(); | |
speed = Serial.read(); | |
speed = min(MAX_SPEED, speed); | |
degree = constrain(degree, MIN_DEGREE[sid], MAX_DEGREE[sid]); | |
servos[sid].write(degree, speed, false); | |
} | |
} | |
delay(20); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment