Login with google-oauth2
Help
ラピロ展示用スケッチ
ラピロの展示をする時などに一定間隔で自動でモーションを切り替えるようにしたスケッチです。 手動でモーションを切り替える手間が省けます。
#define demoInterval 8000 //モーションを切り替える間隔 ms
#define demoStepNum 5 //登録するモーションの数
boolean demoFlag = true;
uint8_t demoMotion[demoStepNum] = {0,5,7,6,8}; //再生するモーションの番号
この値を書き換えてラピロに書き込むことで電源をいれると自動でモーションを切り替えます。
↓スケッチ全体
#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 demoInterval 8000 //ms
#define demoStepNum 5
boolean demoFlag = true;
uint8_t demoMotion[demoStepNum] = {0,5,7,6,8};
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}
}
};
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);
delay(500);
pinMode(POWER, OUTPUT);
digitalWrite(POWER, HIGH);
}
void loop() {
demoLoop();
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;
case 'D':
Serial.print("#D");
while(!Serial.available()){}
buf = Serial.read();
if(buf == 'A'){
demoFlag = true;
Serial.println('A');
Serial.println("Auto demo mode");
}
else if(buf == 'S'){
demoFlag = false;
Serial.println('S');
Serial.println("Stop demo mode");
}
else{
Serial.print('E');
}
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);
}
}
}
//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;
}
//************** Auto Demo *****************//
void demoLoop()
{
long nowTime;
static long demoTime = 0;
static int demoNum = 0;
nowTime = millis();
if((nowTime - demoTime) > demoInterval){
if(demoFlag == true){
motionNumber = demoMotion[demoNum];
//Serial.print("#M");
//Serial.println(motionNumber);
demoNum++;
demoTime = nowTime;
if(demoNum >= demoStepNum){
demoNum = 0;
}
}
}
}