#include "Servo.h"
#define FRAME 20 // interval time from current step to next step: 20msecc
int numberOfStep = 10;
int current_angle[4];
int servo_trim[4]; // trim to adjust each servo motors's angle to center
Servo servo[4];
#define PIN_CENTER 4
#define PIN_RIGHT 6
#define PIN_LEFT 5
#define PIN_NECK 7
#define PIN_DELAY 50
#define MOTOR_CENTER 0
#define MOTOR_RIGHT 1
#define MOTOR_LEFT 2
#define MOTOR_NECK 3
char buf[16];
void initServo()
{
// attach pins to each servo
servo[0].attach(PIN_CENTER);
servo[1].attach(PIN_RIGHT);
servo[2].attach(PIN_LEFT);
servo[3].attach(PIN_NECK);
// set trim to each servo
servo_trim[0] = 0;
servo_trim[1] = 0;
servo_trim[2] = 0;
servo_trim[3] = 0;
current_angle[0] = 0;
current_angle[1] = 0;
current_angle[2] = 0;
current_angle[3] = 0;
// rotate all servos to center position
servo[0].write(90 + servo_trim[0]);
servo[1].write(90 + servo_trim[1]);
servo[2].write(90 + servo_trim[2]);
servo[3].write(90 + servo_trim[3]);
}
void playMotion(int motor, int target_angle)
{
int loop;
// check limit
if(target_angle > 90) {
target_angle = 90;
} else if (target_angle < -90) {
target_angle = -90;
}
float rotating_angle = ((float)target_angle - (float)current_angle[motor]) / (float)numberOfStep;
float tmp_angle = (float)current_angle[motor];
loop = numberOfStep;
while(loop)
{
tmp_angle += rotating_angle;
if (rotating_angle < 0){
if(current_angle[motor] > target_angle) current_angle[motor] = (int)tmp_angle; else current_angle[motor] = target_angle;
} else if(rotating_angle > 0) {
if (current_angle[motor] < target_angle) current_angle[motor] = (int)tmp_angle; else current_angle[motor] = target_angle;
}
servo[motor].write(current_angle[motor] + servo_trim[motor] + 90);
delay(FRAME);
loop--;
}
// adjust current_angle
current_angle[motor] = target_angle;
servo[motor].write(current_angle[motor]+servo_trim[motor]+90);
}
// 遷移時間をフレーム間隔(msec)で割る
void setInterval(int msecInterval)
{
numberOfStep = msecInterval / FRAME; //total number of steps to move to next position
}
void setTrim(int motor, int trim)
{
servo_trim[motor] = trim;
servo[motor].write(servo_trim[motor] + 90);
}
void moveForward()
{
setInterval(200);
playMotion(MOTOR_CENTER, 35); // 左脚を上げる
playMotion(MOTOR_LEFT, -15); // 宙に浮いている左脚を時計回りに捩じる
playMotion(MOTOR_RIGHT, -15); // 右脚を反時計回りに捩じり、宙に浮いている左脚を前に出す
setInterval(600);
playMotion(MOTOR_CENTER, 0); // 左脚を下す
setInterval(200);
playMotion(MOTOR_CENTER, -35); // 右脚を上げる
playMotion(MOTOR_RIGHT, 15); // 宙に浮いている右脚を時計回りに捩じる
playMotion(MOTOR_LEFT, 15); // 左脚を反時計回りに捩じり、宙に浮いている右脚を前に出す
setInterval(600);
playMotion(MOTOR_CENTER, 0); // 右脚を下す
}
void moveBack()
{
setInterval(200);
playMotion(MOTOR_CENTER, -35); // 右脚を上げる
playMotion(MOTOR_RIGHT, -15); // 宙に浮いている右脚を反時計回りに捩じる
playMotion(MOTOR_LEFT, -15); // 左脚を時計回りに捩じる
setInterval(600);
playMotion(MOTOR_CENTER, 0); // 右脚を下す
setInterval(200);
playMotion(MOTOR_CENTER, 35); // 左脚を上げる
playMotion(MOTOR_LEFT, 15); // 宙に浮いている左脚を時計回りに捩じる
playMotion(MOTOR_RIGHT, 15); // 右脚を時計回りに捩じる
setInterval(600);
playMotion(MOTOR_CENTER, 0);
}
void turnLeft()
{
setInterval(200);
playMotion(MOTOR_CENTER, -35);
playMotion(MOTOR_RIGHT, 0);
playMotion(MOTOR_LEFT, 15);
setInterval(600);
playMotion(MOTOR_CENTER, 0);
setInterval(200);
playMotion(MOTOR_CENTER, 35);
playMotion(MOTOR_LEFT, 0);
playMotion(MOTOR_RIGHT, 15);
setInterval(600);
playMotion(MOTOR_CENTER, 0);
}
void turnRight()
{
setInterval(200);
playMotion(MOTOR_CENTER, 35);
playMotion(MOTOR_LEFT, 0);
playMotion(MOTOR_RIGHT, -15);
setInterval(600);
playMotion(MOTOR_CENTER, 0);
setInterval(200);
playMotion(MOTOR_CENTER, -35);
playMotion(MOTOR_RIGHT, 0);
playMotion(MOTOR_LEFT, -15);
setInterval(600);
playMotion(MOTOR_CENTER, 0);
}
void lookAround()
{
setInterval(200);
playMotion(MOTOR_CENTER, 0);
playMotion(MOTOR_RIGHT, 0);
playMotion(MOTOR_LEFT, 0);
playMotion(MOTOR_NECK, 0);
setInterval(300);
playMotion(MOTOR_NECK, 60);
playMotion(MOTOR_NECK, 0);
playMotion(MOTOR_NECK, -60);
playMotion(MOTOR_NECK, 0);
}
void homePosition()
{
if (current_angle[MOTOR_CENTER] > 0) {
playMotion(MOTOR_LEFT, 0);
} else if (current_angle[MOTOR_CENTER] < 0) {
playMotion(MOTOR_RIGHT, 0);
}
if (current_angle[MOTOR_LEFT] != 0) {
playMotion(MOTOR_CENTER, 35);
playMotion(MOTOR_LEFT, 0);
}
if (current_angle[MOTOR_RIGHT] != 0) {
playMotion(MOTOR_CENTER, -35);
playMotion(MOTOR_RIGHT, 0);
}
playMotion(MOTOR_CENTER, 0);
playMotion(MOTOR_NECK, 0);
setInterval(300);
}
void setup()
{
Serial.begin(115200);
initServo();
}
void loop()
{
static int pos = 0;
if(Serial.available()){
char cmd = Serial.read();
switch (cmd){
case 'c': buf[0] = cmd; pos = 1; break;
case 'r': buf[0] = cmd; pos = 1; break;
case 'l': buf[0] = cmd; pos = 1; break;
case 'n': buf[0] = cmd; pos = 1; break;
case 'i': buf[0] = cmd; pos = 1; break; // Interval(msec)
case 0x0a:
buf[pos] = 0x0;
pos = 0;
if (buf[1] == 't') {
switch(buf[0]) {
case 'c': setTrim(MOTOR_CENTER, atoi(&buf[2])); break;
case 'r': setTrim(MOTOR_RIGHT, atoi(&buf[2])); break;
case 'l': setTrim(MOTOR_LEFT, atoi(&buf[2])); break;
case 'n': setTrim(MOTOR_NECK, atoi(&buf[2])); break;
}
} else {
switch (buf[0]) {
case 'c': playMotion(MOTOR_CENTER, atoi(&buf[1])); break;
case 'r': playMotion(MOTOR_RIGHT, atoi(&buf[1])); break;
case 'l': playMotion(MOTOR_LEFT, atoi(&buf[1])); break;
case 'n': playMotion(MOTOR_NECK, atoi(&buf[1])); break;
case 'i': setInterval(atoi(&buf[1])); break;
case 'F': moveForward(); break;
case 'B': moveBack(); break;
case 'L': turnLeft(); break;
case 'R': turnRight(); break;
case 'N': lookAround(); break;
case 'H': homePosition(); break;
}
}
break;
default :
if (pos < 15) {
buf[pos] = cmd;
pos++;
} else {
pos = 0;
}
break;
}
}
delay(10);
}
後継のピッコロボIoTでは、サーボモータ、センシング、ネットワーク通信機能を１枚のボードに搭載しています。
Freaduino に実装されていた3.3V/5V動作電圧切替スイッチはなくなっています。
後継のこのボードは環境設定も含め使い勝手が悪いです。
ちなみにFreaduino V1.2ボードは製造が終了していますが、当時の単品価格は2280円程度でした。
Raspberry Pi（ラズベリー パイ）は、ARMプロセッサを搭載したシングルボードコンピュータ。イギリスのラズベリーパイ財団によって開発されている。
