Scribd
Upload
158 views

Joint Mode Arduino Code

The document defines the code for controlling the motors of a 4-legged robot. It includes definitions for the motor pins of each leg, initialization of the motor controls, and functions for moving the robot like standing, walking, turning and pacing.

Uploaded by

Rohit
Copyright
© © All Rights Reserved
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
158 views

Joint Mode Arduino Code

The document defines the code for controlling the motors of a 4-legged robot. It includes definitions for the motor pins of each leg, initialization of the motor controls, and functions for moving the robot like standing, walking, turning and pacing.

Uploaded by

Rohit
Copyright
© © All Rights Reserved
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
You are on page 1/ 11

#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;

You might also like