よつあしのソースコードをのせておきます。
//==========================================================================================
// 中野島ロボット：〇川 2018.5.5
// mePed_nakarobo_COM.ino
//
// The mePed is an open source quadruped robot designed by Scott Pierce of
// Spierce Technologies (www.meped.io & www.spiercetech.com)
//
// This program is based on code written by Alexey Butov (www.alexeybutov.wix.com/roboset)
//
//==========================================================================================
#include
// (Top View)
//FLONT
// ----- -----
// | 2 | | 8 |
// | P03 | | P09 |
// ----- ----- ----- -----
// | 1 | | 7 |
// | P02 | | P08 |
// ----- -----
// | 3 | | 5 |
// | P04 | | P06 |
// ----- ----- ----- -----
// | 4 | | 6 |
// | P05 | | P07 |
// ----- -----
//BACK
//Arudiroid4 (Top View)
//FLONT
// sp1: Speed 1 sp2: Speed 2 sp3: Speed 3 sp4: Speed 4
//
// ----- -----
// | SP1 | | SP4 |
// ----- -----
// -----
// | Nano|
// | |
// -----
// ----- -----
// | SP2 | | SP3 |
// ----- -----
//BACK
//===== Globals ============================================================================
// calibration
int da = -12; // Left Front Pivot
int db = 18; // Left Back Pivot
int dc = -18; // Right Back Pivot
int dd = 12; // Right Front Pivot
// servo initial positions + calibration
int a90 = (90 + da), a120 = (120 + da), a150 = (150 + da), a180 = (180 + da);
int b0 = (0 + db), b30 = (30 + db), b60 = (60 + db), b90 = (90 + db);
int c90 = (90 + dc), c120 = (120 + dc), c150 = (150 + dc), c180 = (180 + dc);
int d0 = (0 + dd), d30 = (30 + dd), d60 = (60 + dd), d90 = (90 + dd);
// start points for servo
int s11 = 90; // Front Left Pivot Servo
int s12 = 90; // Front Left Lift Servo
int s21 = 90; // Back Left Pivot Servo
int s22 = 90; // Back Left Lift Servo
int s31 = 90; // Back Right Pivot Servo
int s32 = 90; // Back Right Lift Servo
int s41 = 90; // Front Right Pivot Servo
int s42 = 90; // Front Right Lift Servo
int f = 0;
int b = 0;
int l = 0;
int r = 0;
int spd = 3;// Speed of walking motion, larger the number, the slower the speed
int high = 0;// How high the robot is standing
// Define 8 Servos
Servo myServo1; // Front Left Pivot Servo
Servo myServo2; // Front Left Lift Servo
Servo myServo3; // Back Left Pivot Servo
Servo myServo4; // Back Left Lift Servo
Servo myServo5; // Back Right Pivot Servo
Servo myServo6; // Back Right Lift Servo
Servo myServo7; // Front Right Pivot Servo
Servo myServo8; // Front Right Lift Servo
//==========================================================================================
//===== Setup ==============================================================================
void setup() {
// start Serial
Serial.begin (9600);
// Start position
attachServo();
minimam();
detachServo();
}
//setup
//==========================================================================================
//== Loop ==================================================================================
void loop() {
unsigned long value;
unsigned long lastValue;
// Center all servos
high = 15;
// Set hight to 15
spd = 3;
// Set speed to 3
while (Serial.available() > 0) {
//Serial.print(Serial.available());
char value = Serial.read();
Serial.print(value);
attachServo();
switch (value)
{
// MOVE
case '0':
minimam();
break;
case '1':
lastValue = "1";
lean_left();
break;
case '2':
lastValue = "2";
forward();
break;
case '3':
lastValue = "3";
lean_right();
break;
case '4':
lastValue = "4";
turn_right();
break;
case '5':
lastValue = "5";
center_servos();
break;
case '6':
lastValue = "6";
turn_left();
break;
case '7':
lastValue = "7";
step_left();
break;
case '8':
lastValue = "8";
back();
break;
case '9':
lastValue = "9";
step_right();
break;
}
//next value
detachServo();
Serial.print('\n');
delay(50);
// Pause for 50ms before executing next movement
} //while
}//loop
void attachServo()
{
myServo1.attach(2);
myServo2.attach(3);
myServo3.attach(4);
myServo4.attach(5);
myServo5.attach(6);
myServo6.attach(7);
myServo7.attach(8);
myServo8.attach(9);
}
void detachServo()
{
myServo1.detach();
myServo2.detach();
myServo3.detach();
myServo4.detach();
myServo5.detach();
myServo6.detach();
myServo7.detach();
myServo8.detach();
}
//== Lean_Left =============================================================================
void lean_left() {
myServo2.write(40);
myServo4.write(140);
myServo6.write(140);
myServo8.write(40);
delay(700);
}
//== Lean_Right ============================================================================
void lean_right() {
myServo2.write(140);
myServo4.write(40);
myServo6.write(40);
myServo8.write(140);
delay(700);
}
// trim
const int trimStep = 1;
//== trim_left =============================================================================
void trim_left() {
da-=trimStep;
// Left Front Pivot
db-=trimStep;
// Left Back Pivot
dc-=trimStep;
// Right Back Pivot
dd-=trimStep;
// Right Front Pivot
}
//== trim_Right ============================================================================
void trim_right() {
da+=trimStep;
// Left Front Pivot
db+=trimStep;
// Left Back Pivot
dc+=trimStep;
// Right Back Pivot
dd+=trimStep;
// Right Front Pivot
}
//== Forward ===============================================================================
void forward() {
// calculation of points
// Left Front Pivot
a90 = (90 + da), a120 = (120 + da), a150 = (150 + da), a180 = (180 + da);
// Left Back Pivot
b0 = (0 + db), b30 = (30 + db), b60 = (60 + db), b90 = (90 + db);
// Right Back Pivot
c90 = (90 + dc), c120 = (120 + dc), c150 = (150 + dc), c180 = (180 + dc);
// Right Front Pivot
d0 = (0 + dd), d30 = (30 + dd), d60 = (60 + dd), d90 = (90 + dd);
// set servo positions and speeds needed to walk forward one step
// (LFP, LBP, RBP, RFP, LFL, LBL, RBL, RFL, S1, S2, S3, S4)
srv(a180, b0 , c120, d60, 42, 33, 33, 42, 1, 3, 1, 1);
srv( a90, b30, c90, d30, 6, 33, 33, 42, 3, 1, 1, 1);
srv( a90, b30, c90, d30, 42, 33, 33, 42, 3, 1, 1, 1);
srv(a120, b60, c180, d0, 42, 33, 6, 42, 1, 1, 3, 1);
srv(a120, b60, c180, d0, 42, 33, 33, 42, 1, 1, 3, 1);
srv(a150, b90, c150, d90, 42, 33, 33, 6, 1, 1, 1, 3);
srv(a150, b90, c150, d90, 42, 33, 33, 42, 1, 1, 1, 3);
srv(a180, b0, c120, d60, 42, 6, 33, 42, 1, 3, 1, 1);
//
srv(a180, b0, c120, d60, 42, 15, 33, 42, 1, 3, 1, 1);
}
//== Back ==================================================================================
void back () {
// set servo positions and speeds needed to walk backward one step
// (LFP, LBP, RBP, RFP, LFL, LBL, RBL, RFL, S1, S2, S3, S4)
srv(180, 0, 120, 60, 42, 33, 33, 42, 3, 1, 1, 1);
srv(150, 90, 150, 90, 42, 18, 33, 42, 1, 3, 1, 1);
srv(150, 90, 150, 90, 42, 33, 33, 42, 1, 3, 1, 1);
srv(120, 60, 180, 0, 42, 33, 33, 6, 1, 1, 1, 3);
srv(120, 60, 180, 0, 42, 33, 33, 42, 1, 1, 1, 3);
srv(90, 30, 90, 30, 42, 33, 18, 42, 1, 1, 3, 1);
srv(90, 30, 90, 30, 42, 33, 33, 42, 1, 1, 3, 1);
//
// srv(180, 0, 120, 60, 6, 33, 33, 42, 3, 1, 1, 1);
srv(160,20, 120, 60, 18, 33, 33, 42, 3, 1, 1, 1);
}
//== Left =================================================================================
void turn_left () {
// set servo positions and speeds needed to turn left one step
// (LFP, LBP, RBP, RFP, LFL, LBL, RBL, RFL, S1, S2, S3, S4)
srv(150, 90, 90, 30, 42, 6, 33, 42, 1, 3, 1, 1);
srv(150, 90, 90, 30, 42, 33, 33, 42, 1, 3, 1, 1);
srv(120, 60, 180, 0, 42, 33, 6, 42, 1, 1, 3, 1);
srv(120, 60, 180, 0, 42, 33, 33, 24, 1, 1, 3, 1);
srv(90, 30, 150, 90, 42, 33, 33, 6, 1, 1, 1, 3);
srv(90, 30, 150, 90, 42, 33, 33, 42, 1, 1, 1, 3);
srv(180, 0, 120, 60, 6, 33, 33, 42, 3, 1, 1, 1);
srv(180, 0, 120, 60, 42, 33, 33, 33, 3, 1, 1, 1);
}
//== Right ================================================================================
void turn_right () {
// set servo positions and speeds needed to turn right one step
// (LFP, LBP, RBP, RFP, LFL, LBL, RBL, RFL, S1, S2, S3, S4)
srv( 90, 30, 150, 90, 6, 33, 33, 42, 3, 1, 1, 1);
srv( 90, 30, 150, 90, 42, 33, 33, 42, 3, 1, 1, 1);
srv(120, 60, 180, 0, 42, 33, 33, 6, 1, 1, 1, 3);
srv(120, 60, 180, 0, 42, 33, 33, 42, 1, 1, 1, 3);
srv(150, 90, 90, 30, 42, 33, 6, 42, 1, 1, 3, 1);
srv(150, 90, 90, 30, 42, 33, 33, 42, 1, 1, 3, 1);
srv(180, 0, 120, 60, 42, 6, 33, 42, 1, 3, 1, 1);
srv(180, 0, 120, 60, 42, 33, 33, 42, 1, 3, 1, 1);
}
//== Right Step================================================================================
void step_right () {
myServo1.write(130);
myServo3.write(50);
myServo5.write(130);
myServo7.write(50);
delay(500);
myServo2.write(130);
myServo4.write(50);
myServo6.write(40);
myServo8.write(140);
delay(300);
myServo2.write(90);
myServo4.write(90);
myServo6.write(140);
myServo8.write(40);
delay(150);
myServo2.write(130);
myServo4.write(50);
myServo6.write(40);
myServo8.write(140);
delay(700);
}
//== Left Step ================================================================================
void step_left () {
myServo1.write(130);
myServo3.write(50);
myServo5.write(130);
myServo7.write(50);
delay(500);
myServo2.write(40);
myServo4.write(140);
myServo6.write(130);
myServo8.write(50);
delay(300);
myServo2.write(140);
myServo4.write(40);
myServo6.write(90);
myServo8.write(90);
delay(150);
myServo2.write(40);
myServo4.write(140);
myServo6.write(130);
myServo8.write(50);
delay(700);
}
//== Center Servos ========================================================================
void center_servos() {
myServo1.write(90);
myServo3.write(90);
myServo5.write(90);
myServo7.write(90);
delay(500);
myServo2.write(90);
myServo4.write(90);
myServo6.write(90);
myServo8.write(90);
int s11 = 90; // Front Left Pivot Servo
int s12 = 90; // Front Left Lift Servo
int s21 = 90; // Back Left Pivot Servo
int s22 = 90; // Back Left Lift Servo
int s31 = 90; // Back Right Pivot Servo
int s32 = 90; // Back Right Lift Servo
int s41 = 90; // Front Right Pivot Servo
int s42 = 90; // Front Right Lift Servo
delay(500);
}
//== Minimam Servos ========================================================================
void minimam() {
myServo2.write(140);
myServo4.write(40);
myServo6.write(140);
myServo8.write(40);
delay(300);
myServo1.write(140);
myServo3.write(40);
myServo5.write(140);
myServo7.write(40);
delay(700);
}
//== Increase Speed ========================================================================
void increase_speed() {
if (spd > 3) spd--;
}
//== Decrease Speed ========================================================================
void decrease_speed() {
if (spd < 50) spd++;
}
//== Srv ===================================================================================
void srv( int p11, int p21, int p31, int p41, int p12, int p22, int p32, int p42, int sp1, int sp2, int sp3, int sp4) {
// p11: Front Left Pivot Servo
// p21: Back Left Pivot Servo
// p31: Back Right Pivot Servo
// p41: Front Right Pivot Servo
// p12: Front Left Lift Servo
// p22: Back Left Lift Servo
// p32: Back Right Lift Servo
// p42: Front Right Lift Servo
// sp1: Speed 1
// sp2: Speed 2
// sp3: Speed 3
// sp4: Speed 4
// Multiply lift servo positions by manual height adjustment
p12 = p12 + high * 3;
p22 = p22 + high * 3;
p32 = p32 + high * 3;
p42 = p42 + high * 3;
while *1 {
// Front Left Pivot Servo
if (s11 < p11) // if servo position is less than programmed position
{
if ((s11 + sp1) <= p11) s11 = s11 + sp1; // set servo position equal to servo position plus speed constant
else s11 = p11;
}
if (s11 > p11) // if servo position is greater than programmed position
{
if ((s11 - sp1) >= p11) s11 = s11 - sp1; // set servo position equal to servo position minus speed constant
else s11 = p11;
}
// Back Left Pivot Servo
if (s21 < p21) {
if ((s21 + sp2) <= p21) s21 = s21 + sp2 ;
else s21 = p21;
}
if (s21 > p21) {
if ((s21 - sp2) >= p21) s21 = s21 - sp2;
else s21 = p21;
}
// Back Right Pivot Servo
if (s31 < p31) {
if ((s31 + sp3) <= p31) s31 = s31 + sp3;
else s31 = p31;
}
if (s31 > p31) {
if ((s31 - sp3) >= p31) s31 = s31 - sp3;
else s31 = p31;
}
// Front Right Pivot Servo
if (s41 < p41) {
if ((s41 + sp4) <= p41) s41 = s41 + sp4;
else s41 = p41;
}
if (s41 > p41) {
if ((s41 - sp4) >= p41) s41 = s41 - sp4;
else s41 = p41;
}
// Front Left Lift Servo
if (s12 < p12) {
if ((s12 + sp1) <= p12) s12 = s12 + sp1;
else s12 = p12;
}
if (s12 > p12) {
if ((s12 - sp1) >= p12) s12 = s12 - sp1;
else s12 = p12;
}
// Back Left Lift Servo
if (s22 < p22) {
if ((s22 + sp2) <= p22) s22 = s22 + sp2;
else s22 = p22;
}
if (s22 > p22) {
if ((s22 - sp2) >= p22) s22 = s22 - sp2;
else s22 = p22;
}
// Back Right Lift Servo
if (s32 < p32) {
if ((s32 + sp3) <= p32) s32 = s32 + sp3;
else s32 = p32;
}
if (s32 > p32) {
if ((s32 - sp3) >= p32) s32 = s32 - sp3;
else s32 = p32;
}
// Front Right Lift Servo
if (s42 < p42) {
if ((s42 + sp4) <= p42) s42 = s42 + sp4;
else s42 = p42;
}
if (s42 > p42) {
if ((s42 - sp4) >= p42) s42 = s42 - sp4;
else s42 = p42;
}
// Write Pivot Servo Values
myServo1.write(s11 + da);
myServo3.write(s21 + db);
myServo5.write(s31 + dc);
myServo7.write(s41 + dd);
// Write Lift Servos Values
myServo2.write(s12);
myServo4.write(s22);
myServo6.write(s32);
myServo8.write(s42);
delay(spd);
// Delay before next movement
}// while
}//srv
*1:s11 != p11) || (s21 != p21) || (s31 != p31) || (s41 != p41) || (s12 != p12) || (s22 != p22) || (s32 != p32) || (s42 != p42
