#include <Dynamixel2Arduino.

h>

#define BOARD_BUTTON_PIN 23

#if defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit.

#define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on
the OpenCM 9.04 board)

#define DEBUG_SERIAL Serial

const uint8_t DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the
OpenCM 9.04 board)

#endif

//leg1

const uint8_t L1motor1 = 11;

const uint8_t L1motor2 = 12;

const uint8_t L1motor3 = 13;

const uint8_t L1motor4 = 14;

//leg2

const uint8_t L2motor1 = 21;

const uint8_t L2motor2 = 22;

const uint8_t L2motor3 = 23;

const uint8_t L2motor4 = 24;

//leg3

const uint8_t L3motor1 = 31;

const uint8_t L3motor2 = 32;

const uint8_t L3motor3 = 33;

const uint8_t L3motor4 = 34;

//leg4
const uint8_t L4motor1 = 41;

const uint8_t L4motor2 = 42;

const uint8_t L4motor3 = 43;

const uint8_t L4motor4 = 44;

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

//This namespace is required to use Control table item names

using namespace ControlTableItem;

void setup() {

pinMode(BOARD_BUTTON_PIN, INPUT_PULLDOWN);

DEBUG_SERIAL.begin(115200);

dxl.begin(1000000);

dxl.setPortProtocolVersion(2);

//////////////////////////////leg1

//moto1

dxl.ping(L1motor1);

dxl.torqueOff(L1motor1);

dxl.setOperatingMode(L1motor1, OP_POSITION);

dxl.torqueOn(L1motor1);

// Limit the maximum velocity in Position Control Mode. Use 0 for Max speed

//dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 30);

//motor2

dxl.ping(L1motor2);
 dxl.torqueOff(L1motor2);

dxl.setOperatingMode(L1motor2, OP_POSITION);

dxl.torqueOn(L1motor2);

// Limit the maximum velocity in Position Control Mode. Use 0 for Max speed

//dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 30);

//motor3

dxl.ping(L1motor3);

dxl.torqueOff(L1motor3);

dxl.setOperatingMode(L1motor3, OP_POSITION);

dxl.torqueOn(L1motor3);

// Limit the maximum velocity in Position Control Mode. Use 0 for Max speed

//dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 30);

//wheelmotor4

dxl.ping(L1motor4);

dxl.torqueOff(L1motor4);

dxl.setOperatingMode(L1motor4, OP_VELOCITY);

dxl.torqueOn(L1motor4);

/////////////////////////////////

//////////////////leg2

//motor1

dxl.ping(L2motor1);

dxl.torqueOff(L2motor1);

dxl.setOperatingMode(L2motor1, OP_POSITION);

dxl.torqueOn(L2motor1);

// Limit the maximum velocity in Position Control Mode. Use 0 for Max speed

//dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 30);

//motor2

dxl.ping(L2motor2);

dxl.torqueOff(L2motor2);

dxl.setOperatingMode(L2motor2, OP_POSITION);

dxl.torqueOn(L2motor2);

// Limit the maximum velocity in Position Control Mode. Use 0 for Max speed

//dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 30);

//motor3

dxl.ping(L2motor3);

dxl.torqueOff(L2motor3);

dxl.setOperatingMode(L2motor3, OP_POSITION);

dxl.torqueOn(L2motor3);

// Limit the maximum velocity in Position Control Mode. Use 0 for Max speed

//dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 30);

//wheelmotor4

dxl.ping(L2motor4);

dxl.torqueOff(L2motor4);

dxl.setOperatingMode(L2motor4, OP_VELOCITY);

dxl.torqueOn(L2motor4);

///////////----------------------//////////////////////

//////////////////////////////leg3

//moto1

dxl.ping(L3motor1);

dxl.torqueOff(L3motor1);

dxl.setOperatingMode(L3motor1, OP_POSITION);

dxl.torqueOn(L3motor1);

// Limit the maximum velocity in Position Control Mode. Use 0 for Max speed
 //dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 30);

//motor2

dxl.ping(L3motor2);

dxl.torqueOff(L3motor2);

dxl.setOperatingMode(L3motor2, OP_POSITION);

dxl.torqueOn(L3motor2);

// Limit the maximum velocity in Position Control Mode. Use 0 for Max speed

//dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 30);

//motor3

dxl.ping(L3motor3);

dxl.torqueOff(L3motor3);

dxl.setOperatingMode(L3motor3, OP_POSITION);

dxl.torqueOn(L3motor3);

// Limit the maximum velocity in Position Control Mode. Use 0 for Max speed

//dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 30);

//wheelmotor4

dxl.ping(L3motor4);

dxl.torqueOff(L3motor4);

dxl.setOperatingMode(L3motor4, OP_VELOCITY);

dxl.torqueOn(L3motor4);

/////////////////////////////////

//////////////////leg4

//motor1

dxl.ping(L4motor1);

dxl.torqueOff(L4motor1);

dxl.setOperatingMode(L4motor1, OP_POSITION);
 dxl.torqueOn(L4motor1);

// Limit the maximum velocity in Position Control Mode. Use 0 for Max speed

//dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 30);

//motor2

dxl.ping(L4motor2);

dxl.torqueOff(L4motor2);

dxl.setOperatingMode(L4motor2, OP_POSITION);

dxl.torqueOn(L4motor2);

// Limit the maximum velocity in Position Control Mode. Use 0 for Max speed

//dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 30);

//motor3

dxl.ping(L4motor3);

dxl.torqueOff(L4motor3);

dxl.setOperatingMode(L4motor3, OP_POSITION);

dxl.torqueOn(L4motor3);

// Limit the maximum velocity in Position Control Mode. Use 0 for Max speed

//dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 30);

//wheelmotor4

dxl.ping(L4motor4);

dxl.torqueOff(L4motor4);

dxl.setOperatingMode(L4motor4, OP_VELOCITY);

dxl.torqueOn(L4motor4);

///////////////to run code for once.

stand();

delay(2000);

//walk();
 //wheel();

///////////////////////////////end of config

void stand(){

dxl.setGoalPosition(L1motor1, 137, UNIT_DEGREE);

dxl.setGoalPosition(L1motor2, 180, UNIT_DEGREE);

dxl.setGoalPosition(L1motor3, 180, UNIT_DEGREE);

//

dxl.setGoalPosition(L2motor1, 137, UNIT_DEGREE);

dxl.setGoalPosition(L2motor2, 180, UNIT_DEGREE);

dxl.setGoalPosition(L2motor3, 180, UNIT_DEGREE);

//

dxl.setGoalPosition(L3motor1, 137, UNIT_DEGREE);

dxl.setGoalPosition(L3motor2, 180, UNIT_DEGREE);

dxl.setGoalPosition(L3motor3, 180, UNIT_DEGREE);

//

dxl.setGoalPosition(L4motor1, 137, UNIT_DEGREE);

dxl.setGoalPosition(L4motor2, 180, UNIT_DEGREE);

dxl.setGoalPosition(L4motor3, 180, UNIT_DEGREE);

/*

* //delay(400);

DEBUG_SERIAL.print("Present Position(degree) : ");

DEBUG_SERIAL.println(dxl.getPresentPosition(L1motor1));

DEBUG_SERIAL.println(dxl.getPresentPosition(L4motor1));

*/

}
void wheel(){

dxl.setGoalVelocity(L1motor4, 250);

dxl.setGoalVelocity(L2motor4, 250);

dxl.setGoalVelocity(L3motor4, 250);

dxl.setGoalVelocity(L4motor4, 250);

void counterLeft(){

dxl.setGoalVelocity(L1motor4, -60, UNIT_RPM);

dxl.setGoalVelocity(L2motor4, 60, UNIT_RPM);

dxl.setGoalVelocity(L3motor4, 60, UNIT_RPM);

dxl.setGoalVelocity(L4motor4, -60, UNIT_RPM);

void counterRight(){

dxl.setGoalVelocity(L1motor4, 60, UNIT_RPM);

dxl.setGoalVelocity(L2motor4, -60, UNIT_RPM);

dxl.setGoalVelocity(L3motor4, -60, UNIT_RPM);

dxl.setGoalVelocity(L4motor4, 60, UNIT_RPM);

void stay(){

dxl.setGoalPosition(L1motor2, 180, UNIT_DEGREE);

dxl.setGoalPosition(L2motor2, 180, UNIT_DEGREE);

dxl.setGoalPosition(L3motor2, 180, UNIT_DEGREE);

dxl.setGoalPosition(L4motor2, 180, UNIT_DEGREE);

void walk(){

//leg1

dxl.setGoalPosition(L1motor1, 198, UNIT_DEGREE);

 dxl.setGoalPosition(L1motor2, 238, UNIT_DEGREE);

//leg3

dxl.setGoalPosition(L3motor1, 130, UNIT_DEGREE);

dxl.setGoalPosition(L3motor2, 198, UNIT_DEGREE);

//leg2

dxl.setGoalPosition(L2motor1, 130, UNIT_DEGREE);

dxl.setGoalPosition(L2motor2, 198, UNIT_DEGREE);

//leg4

dxl.setGoalPosition(L4motor1, 198, UNIT_DEGREE);

dxl.setGoalPosition(L4motor2, 238, UNIT_DEGREE);

delay(600);

//leg1

dxl.setGoalPosition(L1motor1, 128, UNIT_DEGREE);

dxl.setGoalPosition(L1motor2, 198, UNIT_DEGREE);

//leg3

dxl.setGoalPosition(L3motor1, 198, UNIT_DEGREE);

dxl.setGoalPosition(L3motor2, 240, UNIT_DEGREE);

//leg2

dxl.setGoalPosition(L2motor1, 198, UNIT_DEGREE);

dxl.setGoalPosition(L2motor2, 240, UNIT_DEGREE);

//leg4

dxl.setGoalPosition(L4motor1, 130, UNIT_DEGREE);

dxl.setGoalPosition(L4motor2, 178, UNIT_DEGREE);

delay(600);

void pace(){

dxl.setGoalPosition(L1motor1, 137, UNIT_DEGREE);

int initial_present_position = 0;
 int final_present_position = 180;

while (abs(180 - initial_present_position) > 90)

final_present_position = dxl.getPresentPosition(L1motor1, UNIT_DEGREE);

initial_present_position = dxl.getPresentPosition(L1motor2, UNIT_DEGREE);

delay(800);

dxl.setGoalPosition(L1motor1,220, UNIT_DEGREE);

while (abs(140 - final_present_position) > 90)

final_present_position = dxl.getPresentPosition(L1motor1, UNIT_DEGREE);

initial_present_position = dxl.getPresentPosition(L1motor2);

delay(800);

void amble(){

dxl.setGoalPosition(L2motor1, 135, UNIT_DEGREE);///1

dxl.setGoalPosition(L3motor1, 135, UNIT_DEGREE);///4

dxl.setGoalPosition(L2motor2, 235, UNIT_DEGREE);///knee

dxl.setGoalPosition(L3motor2, 235, UNIT_DEGREE);///kne

delay(100);

dxl.setGoalPosition(L1motor1, 135, UNIT_DEGREE);///1

dxl.setGoalPosition(L4motor1, 135, UNIT_DEGREE);///4

dxl.setGoalPosition(L1motor2, 170, UNIT_DEGREE);///

dxl.setGoalPosition(L4motor2, 170, UNIT_DEGREE);///knee

delay(900);

dxl.setGoalPosition(L2motor2, 170, UNIT_DEGREE);///knee

 dxl.setGoalPosition(L3motor2, 170, UNIT_DEGREE);///kne

dxl.setGoalPosition(L1motor2, 235, UNIT_DEGREE);///

dxl.setGoalPosition(L4motor2, 235, UNIT_DEGREE);///knee

delay(900);

void loop(){

walk();

//amble();

//wheel;