中野島ロボット

小さなロボットの自作記事を書いています。

GW Day7(3/3) 3Dプリンタで4足ロボット(FW編)

よつあしのソースコードをのせておきます。

 

//==========================================================================================

// 中野島ロボット:〇川 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