Hi.
I built an app that is able to send the number 1 or 2 by means of the push of 2 buttons. When Arduino reads 1 then the servo attached did something. When the Arduino reads 2 then the servo will be turned off and doesn't move more.
My servo is a dynamixel. This means that it has a pid behaviour inside itself. What I want is, at this point, to add to the app the possibility to change the parameters of PID before the move of dynamixel. This means that I have to send 3 parameters(P,I,D) in the same time. I did it! But I am not able to add it in the same code where I switch the on/off of the servo with the reading of "1" OR "2".
I hope that I explained myself quite good.
What I can say more is that when the number 1 is sent, then the imu starts to read accelerations and basing on them, the dynamixel did some moves.
#include <SoftwareSerial.h>
SoftwareSerial BTSerial(7,8);
#include <Arduino_LSM6DS3.h>
#include <DynamixelShield.h>
const uint8_t DXL_ID = 1;
const float DXL_PROTOCOL_VERSION = 2.0;
DynamixelShield dxl;
//This namespace is required to use Control table item names
using namespace ControlTableItem;
#define DEBUG_SERIAL Serial
int walk_state=0;
int read_acc=0;
float Object_position_X = 170;
float Object_position_Y = 60;
float Athr = 0.22;
float Bthr = -0.1;
void setup(){
DEBUG_SERIAL.begin(57600);
delay(2000);
BTSerial.begin(57600);
delay(2000);
IMU.begin();
delay(2000);
dxl.begin(57600);
// Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
// Turn off torque when configuring items in EEPROM area
dxl.torqueOff(DXL_ID);
dxl.setOperatingMode(DXL_ID, OP_POSITION);
dxl.torqueOn(DXL_ID);
DEBUG_SERIAL.print("Connect the device via bluetooth") ;
}
String Data;
void loop(){
if (BTSerial.available()>0){
Data=BTSerial.readString();
}
///WALK///
if(walk_state==0 && Data == "1"){
DEBUG_SERIAL.println("FUNZIONE WALKING ABILITATA");
walk_state=1;
read_acc=1;
}
if(read_acc==1){
float x,y,z;
IMU.readAcceleration(x, y, z);
DEBUG_SERIAL.print(y);
DEBUG_SERIAL.print("\t");
if (y>Athr){
dxl.setGoalPosition(DXL_ID, 170,UNIT_DEGREE);
DEBUG_SERIAL.println("stance");
}
else if(y<Bthr){
dxl.setGoalPosition(DXL_ID, 60,UNIT_DEGREE);
DEBUG_SERIAL.println("swing");
}
else {
DEBUG_SERIAL.println();
}
}
if(walk_state==1 && Data == "2"){
DEBUG_SERIAL.println("FUNZIONE WALKING DISABILITATA");
walk_state=0;
read_acc=0;
Data = "nothing";
delay(1000);
dxl.setGoalPosition(DXL_ID, 90,UNIT_DEGREE);
}
////HOW TO ENTER IN THE MODALITY TO SEND PID VALUES////
///In the app I putted a textbox where I write 3 numbers divided ////
///each one by a comma and that have at the end an asterisk. //////
if(Data == "3"){
char caracter="";
String palabra="";
int ind1="";
int ind2="";
int ind3="";
String P="";
String I="";
String D="";
DEBUG_SERIAL.println("Define PID");
delay(1000);
DEBUG_SERIAL.print(Data);
caracter = BTSerial.read();
palabra = palabra + caracter;
DEBUG_SERIAL.print(palabra);
DEBUG_SERIAL.print(caracter);
if(caracter == '*') {
palabra = palabra.substring(0, palabra.length() - 1); // Delete last char *
ind1 = palabra.indexOf(',');
P = palabra.substring(0, ind1);
DEBUG_SERIAL.print("P = ");
DEBUG_SERIAL.println(P.toInt());
ind2 = palabra.indexOf(',', ind1+1 );
I = palabra.substring(ind1+1, ind2);
DEBUG_SERIAL.print("I = ");
DEBUG_SERIAL.println(I.toInt());
ind3 = palabra.indexOf(',', ind2+1 );
D = palabra.substring(ind2+1);
DEBUG_SERIAL.print("D = ");
DEBUG_SERIAL.println(D.toInt());
dxl.writeControlTableItem(POSITION_P_GAIN, DXL_ID, P.toInt());
dxl.writeControlTableItem(POSITION_I_GAIN, DXL_ID, I.toInt());
dxl.writeControlTableItem(POSITION_D_GAIN, DXL_ID, D.toInt());
}
caracter="";
palabra="";
}
}