Home e-Manual

Drive a dynamixel wireless

Hello people,

I am quite a rookie in C programming. At this moment I created 1 sketch that can understand the thresholds that I put in Imu acceleration of my OpenCR1.0. Starting from them, the dynamixel mx64AT will do some movements with different set currents. At the same time, it writes positions in the serial monitor.
I created another sketch that builds communication between the serial of pc and the serial of the module ESP8266-01, connecting firstly to my hotspot.

What I want to understand is how I can have the results of my first sketch using no more the USB cable connected to pc.
I want to record all positions wireless. How can I do?

FIRST SKETCH
#include <Dynamixel2Arduino.h>
// Please modify it to suit your hardware.
#if defined(ARDUINO_OpenCR)
#define DXL_SERIAL Serial3
#define DEBUG_SERIAL Serial
const uint8_t DXL_DIR_PIN = 84; // OpenCR Board’s DIR PIN.
#endif
const uint8_t DXL_ID = 1;
const float DXL_PROTOCOL_VERSION = 2.0;
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
//This namespace is required to use Control table item names
using namespace ControlTableItem;

#include <IMU.h>
cIMU IMU;
#define BDPIN_LED_USER_1 22
#define BDPIN_LED_USER_2 23
#define BDPIN_LED_USER_3 24
#define BDPIN_LED_USER_4 25
int led_pin_user[4] = { BDPIN_LED_USER_1, BDPIN_LED_USER_2, BDPIN_LED_USER_3, BDPIN_LED_USER_4 };

void setup() {
// Use UART port of DYNAMIXEL Shield to debug.
DEBUG_SERIAL.begin(115200);
while(!DEBUG_SERIAL);
// Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
dxl.begin(57600);
// Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
// Get DYNAMIXEL information
dxl.ping(DXL_ID);

IMU.begin();
pinMode(led_pin_user[0], OUTPUT);
pinMode(led_pin_user[3], OUTPUT);

}

void loop() {
//GET REAL TIME POSITION
int i_present_position = 0;
float f_present_position = 0.0;

f_present_position = dxl.getPresentPosition(DXL_ID, UNIT_DEGREE);
i_present_position = dxl.getPresentPosition(DXL_ID);
DEBUG_SERIAL.print("Pos : ");
DEBUG_SERIAL.print(i_present_position);
DEBUG_SERIAL.print("   ");
DEBUG_SERIAL.print("Deg : ");
DEBUG_SERIAL.println(f_present_position);

//ANALYZING IMU
static uint32_t tTime[3];
static uint32_t imu_time = 0;
if( (millis()-tTime[0]) >= 500 ) tTime[0] = millis();
tTime[2] = micros();
if( IMU.update() > 0 ) imu_time = micros()-tTime[2];
if( (millis()-tTime[1]) >= 50) tTime[1] = millis();

//IMU_X
int Object_position_X = 4000;
if ( IMU.accRaw[0]>11000){

digitalWrite(led_pin_user[0], LOW);
   // Turn off torque when configuring items in EEPROM
dxl.torqueOff(DXL_ID);

// dxl.setOperatingMode(DXL_ID, OP_CURRENT_BASED_POSITION);
dxl.setGoalCurrent(DXL_ID, 100);
dxl.torqueOn(DXL_ID);
// dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 500);
dxl.setGoalPosition(DXL_ID,Object_position_X);

int I_present_position_x = 0;
float F_present_position_x = 0.0;
F_present_position_x = dxl.getPresentPosition(DXL_ID, UNIT_DEGREE);
I_present_position_x = dxl.getPresentPosition(DXL_ID);
DEBUG_SERIAL.println(“STANCE”);
DEBUG_SERIAL.print("Pos : “);
DEBUG_SERIAL.print(I_present_position_x);
DEBUG_SERIAL.print(” ");
DEBUG_SERIAL.print("Deg : ");
DEBUG_SERIAL.println(F_present_position_x);
}
if (IMU.accRaw[0]<11000 ) { //(Object_position_X-3)<I_present_position_x<(Object_position_X+3)
dxl.torqueOff(DXL_ID);
digitalWrite(led_pin_user[0], HIGH);
}

//IMU_Y
int Object_position_Y = 1500;
if ( IMU.accRaw[1]>11000){
int j;
digitalWrite(led_pin_user[3], LOW);
// Turn off torque when configuring items in EEPROM
dxl.torqueOff(DXL_ID);
// dxl.setOperatingMode(DXL_ID, OP_CURRENT_BASED_POSITION);
dxl.setGoalCurrent(DXL_ID, 200);
dxl.torqueOn(DXL_ID);
// dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 500);
dxl.setGoalPosition(DXL_ID,Object_position_Y);

int I_present_position_y = 0;
float F_present_position_y= 0.0;
F_present_position_y = dxl.getPresentPosition(DXL_ID, UNIT_DEGREE);
I_present_position_y = dxl.getPresentPosition(DXL_ID);
for( j=0; j<1; j++ )DEBUG_SERIAL.println(“SWING”);
DEBUG_SERIAL.print("Pos : “);
DEBUG_SERIAL.print(I_present_position_y);
DEBUG_SERIAL.print(” ");
DEBUG_SERIAL.print("Deg : ");
DEBUG_SERIAL.println(F_present_position_y);
}
if (IMU.accRaw[1]<11000 ) { //(Object_position_Y-3)<I_present_position_y<(Object_position_Y+3)
dxl.torqueOff(DXL_ID);
digitalWrite(led_pin_user[3], HIGH);
}
}

SECOND SKETCH
`void setup() {
//Initialize USBserial and UARTserial1 and wait for port to open:
Serial.begin(115200);
Serial1.begin(115200);
while (!Serial) {
; // wait for serial port to connect. Needed for native USB port only
}

// prints title with ending line break
Serial.println(“Activating ESP01…”);
Serial1.println(“AT”);
delay(500);
// Serial1.println(“AT+CWQAP”); //Si disconnette per sicurezza da ciò a cui è connesso
// delay(5000);
Serial1.println(“AT+CWMODE=1”); //1=STA station (il modulo è un client che si connette 2=AP (è un access point, un router) 3=both(ibrido)
Serial1.println(“AT+CWJAP=“SSID”,“PSW””);
}

void loop() {
if (Serial1.available()>0){
Serial.write(Serial1.read());
}
if (Serial.available()>0){
Serial1.write(Serial.read());
}
}`

Thank you for any kind of help

Hi @DjangoTango, thank you for your post and for sharing this question on our Community Page.

Allow me to request more information for my understanding and recommend some contacts who may be able to help.

  1. For my understanding, you are running “FIRST SKETCH” on the OpenCR1.0, which is directly connected to and controlling your DYNAMIXELs (based on IMU, etc) correct?

  2. The “SECOND SKETCH” is running on the ESP8266, and you’d like the ESP8266 to wirelessly receive position from your DYNAMIXELs and print them to the screen.

Does this summarize things correctly? One detail I didn’t notice is- how are the ESP8266 and the OpenCR1.0 connected?

While I’m not very experienced with custom programming, or ESP boards, allow me to tag @willsonand, @Yogurt_Man, and possibly @roboteer who may be able to provide the best suggestions.

Hi, @DjangoTango

The OpenCR 1.0 has the UART port. You could get values from the OpenCR over the serial communication via this port.

@DjangoTango,
I have not used the OpenCR 1.0 yet, but from my experiences with CM-550+RPi4B+PC whereas I had similar needs to yours, I concur with the suggestion by Yogurt_Man to use the UART Port with a BT-210 (for example) and to set up your DEBUG_SERIAL device to point to the UART Port. Then you should be able to use Functions “DEBUG_SERIAL.print()” or “serial.write()” as before, and the data packets would then go out wirelessly via BT-210 to the PC. Of course you will need some kind of Serial Communications App on the PC side to receive this data flow. One thing I found out, the hard way, was that this Data Stream was NOT happening in “real-time”, usually with 2-3 seconds of time delay, not suitable for any “control” usage at all at the PC level.