util.h (1216B)
1 #define GREEN_LED 7 2 #define YELLOW_LED 12 3 #define RED_LED 13 4 5 #define BUTTONA 4 6 #define BUTTONB 2 7 8 #define SERVOA_PIN 6 9 #define SERVOB_PIN 5 10 11 #define SERVOA_ZERO 96 12 #define SERVOB_ZERO 96 13 14 #define SIZE(arr) (sizeof(arr) / sizeof(arr[0])) 15 16 Servo servoA; 17 Servo servoB; 18 19 void 20 setupRobot() { 21 Serial.begin(9800); 22 pinMode(GREEN_LED, OUTPUT); 23 pinMode(YELLOW_LED, OUTPUT); 24 pinMode(RED_LED, OUTPUT); 25 pinMode(BUTTONA, INPUT); 26 pinMode(BUTTONB, INPUT); 27 pinMode(SERVOA_PIN, OUTPUT); 28 pinMode(SERVOB_PIN, OUTPUT); 29 servoA.attach(SERVOA_PIN); 30 servoB.attach(SERVOB_PIN); 31 servoA.write(SERVOA_ZERO); 32 servoB.write(SERVOB_ZERO); 33 } 34 35 void 36 setLEDs(int g, int y, int r) { 37 digitalWrite(GREEN_LED, g); 38 digitalWrite(YELLOW_LED, y); 39 digitalWrite(RED_LED, r); 40 } 41 42 void 43 waitButton(int pin, int state) { 44 while (digitalRead(pin) == state) ; 45 } 46 47 void 48 debounce(int pin) { 49 waitButton(pin, LOW); 50 delay(20); 51 waitButton(pin, HIGH); 52 delay(20); 53 } 54 55 void 56 SEprintf(const char *fmt, ...) { 57 static char sbuf[64] = { 0 }; 58 va_list ap; 59 60 va_start(ap, fmt); 61 vsnprintf(sbuf, 64, fmt, ap); 62 va_end(ap); 63 64 Serial.print(sbuf); 65 } 66 67 int 68 readint() { 69 while (!Serial.available()) ; 70 int i = Serial.parseInt(); 71 SEprintf("\n"); 72 return i; 73 }