Skip to content

Instantly share code, notes, and snippets.

@hunandy14

hunandy14/Ripiro_setter_ino.ino

Last active May 18, 2016 11:26
Show Gist options
  • Save hunandy14/9b263605b81d59a5760cd05c00f59863 to your computer and use it in GitHub Desktop.
Save hunandy14/9b263605b81d59a5760cd05c00f59863 to your computer and use it in GitHub Desktop.
Download ZIP
調整機器人用代碼
Raw
Ripiro_setter_ino.ino
/*
Name :Ripiro_setter.ino
By :Charlotte.HonG
Date :2016/05/17
Fimal :2016/05/18
Update:https://goo.gl/bqZ9Nw
注意：
1. 設定時如果沒有反應可能需關閉電源，設定好再開啟電源測試
2. 如想關閉功能，請將setter_active設定為0
(不確定是否會影響到原生命令列)
3. 偶爾數據顯示會怪怪的，不影響使用(應該是RxTx被Pi占用的關係)
命令說明：
pri => 印出目前數值
* => 一次輸入全部如[0,0,0,0,0,0,0,0,0,0,0,0]
m0~11 => 選擇哪個馬達，如第一個(頭) [m0]
Value => 然後再輸入數值[10]
馬達動作參考：
[M0] (+左轉頭)
[M1] (+左轉腰)
[M2] (+右手上舉)
[M3] (-右手外舉)
[M4] (-右手握緊)
[M5] (-左手上舉)
[M6] (+左手外舉)
[M7] (+左手握緊)
[M8] (+右腳順時針轉)
[M9] (+右腳掌外翹)
[M10] (+左腳順時針轉)
[M11] (+左腳掌內翹)
*/
// by ShotaIshiwatari is licensed under the Creative Commons - Public Domain Dedication license.
#include <Servo.h>
#define SHIFT 7
#define R 0 // Red LED
#define G 1 // Green LED
#define B 2 // Blue LED
#define TIME 15 // Column of Time
#define MAXSN 12 // Max Number of Servos
#define MAXMN 10 // Max Number of Motions
#define MAXFN 8 // Max Number of Frames
#define POWER 17 // Servo power supply control pin
#define ERR -1 // Error
#define setter_active 1
char str[32] = "";
int ptr[MAXSN];
int *array;
int i = 0;
int t = 1;
Servo servo[MAXSN];
uint8_t eyes[3] = { 0, 0, 0};
// Fine angle adjustments (degrees)
int trim[MAXSN] = { 0, // Head yaw
0, // Waist yaw
0, // R Sholder roll
0, // R Sholder pitch
0, // R Hand grip
0, // L Sholder roll
0, // L Sholder pitch
0, // L Hand grip
0, // R Foot yaw
0, // R Foot pitch
0, // L Foot yaw
0
}; // L Foot pitch
int nowAngle[MAXSN] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; // Initialize array to 0
int targetAngle[MAXSN] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; // Initialize array to 0
int deltaAngle[MAXSN] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; // Initialize array to 0
uint8_t bufferAngle[MAXSN] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; // Initialize array to 0
uint8_t tempAngle[MAXSN] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; // Initialize array to 0
int nowBright[3] = { 0, 0, 0}; // Initialize array to 0
int targetBright[3] = { 0, 0, 0}; // Initialize array to 0
int deltaBright[3] = { 0, 0, 0}; // Initialize array to 0
uint8_t bufferBright[3] = { 0, 0, 0}; // Initialize array to 0
uint8_t tempBright[3] = { 0, 0, 0}; // Initialize array to 0
double startTime = 0; // Motion start time(msec)
double endTime = 0; // Motion end time(msec)
int remainingTime = 0; // Motion remaining time(msec)
uint8_t bufferTime = 0; // Motion buffer time (0.1sec)
uint8_t motionNumber = 0;
uint8_t frameNumber = 0;
char mode = 'M';
uint8_t motion[MAXMN][MAXFN][16] = {
{ // 0 Stop
{ 90, 90, 0, 130, 90, 180, 50, 90, 90, 90, 90, 90, 0, 0, 255, 10},
{ 90, 90, 0, 130, 90, 180, 50, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 130, 90, 180, 50, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 130, 90, 180, 50, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 130, 90, 180, 50, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 130, 90, 180, 50, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 130, 90, 180, 50, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 130, 90, 180, 50, 90, 90, 90, 90, 90, 0, 0, 0, 0}
},
{ // 1 Forward
{ 90, 90, 0, 90, 90, 180, 90, 90, 80, 110, 80, 120, 0, 0, 0, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 70, 90, 70, 90, 0, 0, 255, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 70, 70, 70, 80, 0, 0, 255, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 100, 60, 100, 70, 0, 0, 0, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 110, 90, 110, 90, 0, 0, 255, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 110, 100, 110, 110, 0, 0, 255, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0}
},
{ // 2 Back
{ 90, 90, 0, 90, 90, 180, 90, 90, 100, 110, 100, 120, 0, 0, 0, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 110, 90, 110, 90, 0, 0, 255, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 110, 70, 110, 80, 0, 0, 255, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 80, 30, 80, 70, 0, 0, 0, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 70, 90, 70, 90, 0, 0, 255, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 70, 100, 70, 110, 0, 0, 255, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0}
},
{ // 3 Right
{ 90, 90, 0, 90, 90, 180, 90, 90, 95, 110, 85, 120, 0, 0, 0, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 100, 90, 80, 90, 0, 0, 255, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 100, 70, 80, 80, 0, 0, 0, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 85, 60, 95, 70, 0, 0, 255, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 80, 90, 100, 90, 0, 0, 0, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 80, 100, 100, 110, 0, 0, 255, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0}
},
{ // 4 Left
{ 90, 90, 0, 90, 90, 180, 90, 90, 95, 60, 85, 70, 0, 0, 0, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 100, 90, 80, 90, 0, 0, 255, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 100, 100, 80, 110, 0, 0, 0, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 85, 110, 95, 120, 0, 0, 255, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 80, 90, 100, 90, 0, 0, 0, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 80, 70, 100, 80, 0, 0, 255, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0}
},
{ // 5 Green
{ 90, 90, 120, 90, 90, 60, 90, 90, 90, 90, 90, 90, 0, 0, 0, 10},
{100, 90, 120, 130, 110, 60, 50, 70, 90, 90, 90, 90, 0, 255, 0, 5},
{ 90, 90, 120, 90, 90, 60, 90, 90, 90, 90, 90, 90, 0, 255, 0, 5},
{ 80, 90, 120, 130, 110, 60, 50, 70, 90, 90, 90, 90, 0, 0, 0, 5},
{ 90, 90, 120, 90, 90, 60, 90, 90, 90, 90, 90, 90, 0, 255, 0, 10},
{ 90, 90, 120, 130, 110, 60, 50, 70, 90, 90, 90, 90, 0, 255, 0, 5},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0}
},
{ // 6 Yellow
{ 90, 120, 120, 130, 90, 180, 90, 90, 90, 90, 90, 90, 255, 255, 0, 7},
{ 90, 120, 120, 90, 90, 180, 90, 90, 90, 90, 90, 90, 255, 255, 0, 7},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0}
},
{ // 7 Blue
{ 90, 90, 120, 130, 70, 60, 50, 110, 90, 90, 90, 90, 0, 0, 255, 10},
{ 90, 90, 120, 130, 110, 60, 50, 70, 90, 90, 90, 90, 0, 0, 255, 5},
{ 90, 90, 120, 130, 70, 60, 50, 110, 90, 90, 90, 90, 0, 0, 255, 5},
{ 90, 90, 120, 130, 110, 60, 50, 70, 90, 90, 90, 90, 0, 0, 255, 5},
{ 90, 90, 120, 130, 110, 60, 50, 70, 90, 90, 90, 90, 0, 0, 255, 15},
{ 90, 90, 90, 130, 110, 90, 50, 70, 90, 90, 90, 90, 0, 0, 255, 3},
{ 90, 90, 120, 130, 110, 60, 50, 70, 90, 90, 90, 90, 0, 0, 255, 3},
{ 90, 90, 90, 130, 110, 90, 50, 70, 90, 90, 90, 90, 0, 0, 255, 3}
},
{ // 8 Red
{ 90, 60, 0, 90, 90, 60, 50, 90, 90, 90, 90, 90, 255, 0, 0, 7},
{ 90, 60, 0, 90, 90, 60, 90, 90, 90, 90, 90, 90, 255, 0, 0, 7},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0}
},
{ // 9 Push
{ 90, 90, 90, 130, 110, 180, 50, 90, 90, 90, 90, 90, 0, 0, 0, 10},
{ 90, 90, 90, 130, 110, 180, 50, 90, 90, 90, 90, 90, 0, 0, 255, 5},
{ 90, 90, 90, 130, 110, 180, 50, 90, 90, 90, 90, 90, 0, 0, 255, 25},
{ 90, 90, 90, 130, 90, 180, 50, 90, 90, 90, 90, 90, 0, 0, 0, 5},
{ 40, 140, 90, 70, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 255, 10},
{ 40, 140, 90, 70, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 255, 25},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0},
{ 90, 90, 0, 90, 90, 180, 90, 90, 90, 90, 90, 90, 0, 0, 0, 0}
}
};
char readme[] = "[ReadMe = https://goo.gl/bqZ9Nw]";
char wel[] = "Welocom Ripiro_setter By:Charlotte.HonG";
void setup() {
servo[0].attach(10); // Head yaw
servo[1].attach(11); // Waist yaw
servo[2].attach(9); // R Sholder roll
servo[3].attach(8); // R Sholder pitch
servo[4].attach(7); // R Hand grip
servo[5].attach(12); // L Sholder roll
servo[6].attach(13); // L Sholder pitch
servo[7].attach(14); // L Hand grip
servo[8].attach(4); // R Foot yaw
servo[9].attach(2); // R Foot pitch
servo[10].attach(15); // L Foot yaw
servo[11].attach(16); // L Foot pitch
eyes[R] = 6; // Red LED of eyes
eyes[G] = 5; // Green LED of eyes
eyes[B] = 3; // Blue LED of eyes
for ( i = 0; i < MAXSN; i++) {
targetAngle[i] = motion[0][0][i] << SHIFT;
nowAngle[i] = targetAngle[i];
servo[i].write((nowAngle[i] >> SHIFT) + trim[i]);
}
for (i = 0; i < 3; i++) {
targetBright[i] = 0 << SHIFT;
nowBright[i] = targetBright[i];
analogWrite(eyes[i], nowBright[i] >> SHIFT);
}
Serial.begin(57600);
Serial.println(readme);
// Serial.println(wel);
print_arr(trim);
delay(500);
pinMode(POWER, OUTPUT);
digitalWrite(POWER, HIGH);
}
void loop() {
if (setter_active==1){
ScanSerialStr();
}
int buf = ERR;
if (Serial.available()) {
if (Serial.read() == '#') {
while (!Serial.available()) {}
switch (Serial.read()) {
case 'M':
buf = readOneDigit();
if (buf != ERR) {
motionNumber = buf;
mode = 'M';
digitalWrite(POWER, HIGH);
Serial.print("#M");
Serial.print(motionNumber);
} else {
Serial.print("#EM");
}
break;
case 'P':
buf = getPose();
if (buf != ERR) {
mode = 'P';
digitalWrite(POWER, HIGH);
Serial.print("#PT");
printThreeDigit(buf);
} else {
Serial.print("#EP");
}
break;
case 'Q':
Serial.print("#Q");
if (mode == 'M') {
Serial.print("M");
Serial.print(motionNumber);
Serial.print("T");
buf = (endTime - millis()) / 100;
if (buf < 0) {
buf = 0;
}
printThreeDigit(buf);
}
if (mode == 'P') {
Serial.print("PT");
buf = (endTime - millis()) / 100;
if (buf < 0) {
buf = 0;
}
printThreeDigit(buf);
}
break;
case 'C':
Serial.print("#C");
if (bufferTime > 0) {
Serial.print("F");
} else {
Serial.print("0");
}
break;
default:
Serial.print("#E");
break;
}
}
}
if (endTime > millis()) {
remainingTime = (endTime - millis()) / 10;
for ( i = 0; i < MAXSN; i++) {
nowAngle[i] = targetAngle[i] - (deltaAngle[i] * remainingTime);
servo[i].write((nowAngle[i] >> SHIFT) + trim[i]);
}
for ( i = 0; i < 3; i++) {
nowBright[i] = targetBright[i] - (deltaBright[i] * remainingTime);
analogWrite(eyes[i], nowBright[i] >> SHIFT);
}
} else if (mode == 'M') {
nextFrame();
} else if (mode == 'P') {
if (bufferTime > 0) {
nextPose();
} else if (endTime + 500 < millis()) {
//digitalWrite(POWER, LOW);
}
}
}
/*====================================================================================*/
int* str_split(const char* str, const char* sp) {
char temp[4];
// ¶}©l¤Á³Î
int tempflag = 0, index = 0;
memset( temp, 0, strlen(temp) );
for (i = 0; i <= strlen(str); ++i) {
if ( str[i] == sp[0] || i == strlen(str)) {
tempflag = 0;
// printf("temp=%c%c\n",temp[0],temp[1] );
sscanf(temp, "%d", (ptr + index));
index++;
memset( temp, 0, strlen(temp) );
}
else {
temp[tempflag++] = str[i];
}
}
return ptr;
}
int ch_m = -1, menu_active = 1;
int motor_n = 0;
void ScanSerialStr() {
if (Serial.available()) {
int strnum = 0;
memset( str, 0, strlen(str) );
while (Serial.available() > 0) {
str[strnum++] = Serial.read();
delay(3);
}
/* Arduino¦ */
// Serial.print("Arduino Input :");
// Serial.println(str);
/*Main Menu*/
if (strcmp(str, "*") == 0) {
Serial.println("Seting all.");
ch_m = 0;
return;
} else if (strcmp(str, "pri") == 0) {
Serial.println("Print trim.");
print_arr(trim);
return;
} else if (strcmp(str, "m0") == 0) {
Serial.println("m0");
ch_m = 1;
motor_n = 0;
return;
} else if (strcmp(str, "m1") == 0) {
Serial.println("m1");
ch_m = 1;
motor_n = 1;
return;
} else if (strcmp(str, "m2") == 0) {
Serial.println("m2");
ch_m = 1;
motor_n = 2;
return;
} else if (strcmp(str, "m3") == 0) {
Serial.println("m3");
ch_m = 1;
motor_n = 3;
return;
} else if (strcmp(str, "m4") == 0) {
Serial.println("m4");
ch_m = 1;
motor_n = 4;
return;
} else if (strcmp(str, "m5") == 0) {
Serial.println("m5");
ch_m = 1;
motor_n = 5;
return;
} else if (strcmp(str, "m6") == 0) {
Serial.println("m6");
ch_m = 1;
motor_n = 6;
return;
} else if (strcmp(str, "m7") == 0) {
Serial.println("m7");
ch_m = 1;
motor_n = 7;
return;
} else if (strcmp(str, "m8") == 0) {
Serial.println("m8");
ch_m = 1;
motor_n = 8;
return;
} else if (strcmp(str, "m9") == 0) {
Serial.println("m9");
ch_m = 1;
motor_n = 9;
return;
} else if (strcmp(str, "m10") == 0) {
Serial.println("m10");
ch_m = 1;
motor_n = 10;
return;
} else if (strcmp(str, "m11") == 0) {
Serial.println("m11");
ch_m = 1;
motor_n = 11;
return;
}
/* Seting */
if (ch_m == 0) {
array = str_split(str, ",");
input(array);
print_arr(trim);
} else if( ch_m == 1 ){
array = str_split(str, ",");
input(array[0],motor_n);
print_arr(trim);
}
/* Scan END */
}
}
void input(int data, int n) {
trim[n] = data;
}
void print_arr(int* array) {
Serial.print("trim=[");
for (i = 0; i < MAXSN - 1; ++i) {
trim[i] = array[i];
Serial.print(trim[i]);
Serial.print(",");
} trim[MAXSN - 1] = array[MAXSN - 1];
Serial.print(trim[MAXSN - 1]);
Serial.println("]");
}
void input(int* array) {
for (i = 0; i < MAXSN - 1; ++i) {
trim[i] = array[i];
} trim[MAXSN - 1] = array[MAXSN - 1];
}
/*====================================================================================*/
//Motion Play
void nextFrame() {
frameNumber++;
if (frameNumber >= MAXFN) {
frameNumber = 0;
}
for (i = 0; i < MAXSN; i++) {
bufferAngle[i] = motion[motionNumber][frameNumber][i];
}
for ( i = 0; i < 3; i++) {
bufferBright[i] = motion[motionNumber][frameNumber][MAXSN + i];
}
bufferTime = motion[motionNumber][frameNumber][TIME];
nextPose();
}
//Make a pose
int nextPose() {
if (bufferTime > 0) {
for (i = 0; i < MAXSN; i++) {
targetAngle[i] = bufferAngle[i] << SHIFT;
deltaAngle[i] = ((bufferAngle[i] << SHIFT) - nowAngle[i]) / (bufferTime * 10);
}
for ( i = 0; i < 3; i++) {
targetBright[i] = bufferBright[i] << SHIFT;
deltaBright[i] = ((bufferBright[i] << SHIFT) - nowBright[i]) / (bufferTime * 10);
}
} else {
for (i = 0; i < MAXSN; i++) {
deltaAngle[i] = 0;
}
for (i = 0; i < 3; i++) {
deltaBright[i] = 0;
}
}
startTime = millis();
endTime = startTime + (bufferTime * 100);
bufferTime = 0;
}
//get buffer values of the next pose from serial data
int getPose() {
int buf = 0;
int value = 0;
int maximum = 255;
boolean readPose = true;
if (bufferTime == 0) {
//Initialize array to target angle
for (i = 0; i < MAXSN; i++) {
tempAngle[i] = bufferAngle[i];
}
for ( i = 0; i < 3; i++) {
tempBright[i] = bufferBright[i];
}
} else {
buf = ERR;
readPose = false;
}
//Read data
while (readPose) {
while (!Serial.available()) {}
switch (Serial.read()) {
case 'S':
buf = readOneDigit();
if (buf != ERR) {
value = buf * 10;
buf = readOneDigit();
if (buf != ERR) {
value += buf;
if (0 <= value && value < MAXSN) {
while (!Serial.available()) {}
if (Serial.read() == 'A') {
maximum = 180;
buf = readThreeDigit(maximum);
if (buf != ERR) {
tempAngle[value] = buf;
} else {
readPose = false;
}
} else {
buf = ERR;
readPose = false;
}
} else {
buf = ERR;
readPose = false;
}
}
}
break;
case 'R':
maximum = 255;
buf = readThreeDigit(maximum);
if (buf != ERR) {
tempBright[R] = buf;
} else {
readPose = false;
}
break;
case 'G':
maximum = 255;
buf = readThreeDigit(maximum);
if (buf != ERR) {
tempBright[G] = buf;
} else {
readPose = false;
}
break;
case 'B':
maximum = 255;
buf = readThreeDigit(maximum);
if (buf != ERR) {
tempBright[B] = buf;
} else {
readPose = false;
}
break;
case 'T':
maximum = 255;
buf = readThreeDigit(maximum);
if (buf > 0) {
bufferTime = buf;
for (i = 0; i < MAXSN; i++) {
bufferAngle[i] = tempAngle[i];
}
for ( i = 0; i < 3; i++) {
bufferBright[i] = tempBright[i];
}
}
readPose = false;
break;
default:
buf = ERR;
readPose = false;
break;
}
}
return buf;
}
int printThreeDigit(int buf) {
String s = String(buf);
if (s.length() == 2) {
Serial.print("0");
} else if (s.length() == 1) {
Serial.print("00");
}
Serial.print(s);
}
int digit;
//Read ASCII Three-digit
int readThreeDigit(int maximum) {
int buf;
buf = readOneDigit();
if (buf != ERR) {
digit = buf * 100;
buf = readOneDigit();
if (buf != ERR) {
digit += buf * 10;
buf = readOneDigit();
if (buf != ERR) {
digit += buf;
if (digit <= maximum) {
buf = digit;
} else {
buf = ERR;
}
}
}
}
return buf;
}
//Read ASCII One-digit
int readOneDigit() {
int buf;
while (!Serial.available()) {}
buf = Serial.read() - 48;
if (buf < 0 || 9 < buf) {
buf = ERR;
}
return buf;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment