Good morning
Am I on track for the mini?
Do you have an example of code to control the arm movement in the loop with the keyboard because I don’t know how to do it. I thank you in advance.
Sincerely
`<// **** Paramètres globaux pour les DXL *********
#include <Dynamixel2Arduino.h>
#define DXL_SERIAL Serial1
#define DEBUG_SERIAL Serial
// **** Global Parameters for DXLs *********
const uint8_t BROADCAST_ID = 254;
const uint8_t DXL_ID1 = 1 ;
const uint8_t DXL_ID2 = 2 ;
const uint8_t DXL_ID3 = 3 ;
const uint8_t DXL_ID4 = 4 ;
const uint8_t DXL_ID5 = 5 ;
const uint8_t DXL_ID6 = 6 ;
const uint8_t DXL_ID7 = 7 ;
const uint8_t DXL_ID8 = 8 ;
const uint8_t DXL_ID9 = 9 ;
const uint8_t DXL_ID10 = 10 ;
const uint8_t DXL_ID11 = 11 ;
const uint8_t DXL_ID12 = 12 ;
const uint8_t DXL_ID13 = 13 ;
const uint8_t DXL_ID14 = 14 ;
const uint8_t DXL_ID15 = 15 ;
const uint8_t DXL_ID16 = 16 ;
const float DXL_PROTOCOL_VERSION = 2.0;
const uint8_t GP_DXL_CNT = 16; // nombre de DXL utilisés en mode Contrôle de position
const uint8_t GP_DXL_ID_LIST[GP_DXL_CNT] = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16}; //ID de servo définis en mode Contrôle de position
const uint16_t ADDR_X_PRESENT_POSITION = 37; // (XL-320)
const uint16_t LEN_X_PRESENT_POSITION = 2 ; // (XL-320)
const uint16_t ADDR_X_GOAL_POSITION = 30; // (XL-320)
const uint16_t LEN_X_GOAL_POSITION = 2; // (XL-320)
const uint16_t ADDR_X_MOVING_SPEED= 32; // (XL-320) adress 32 byt 2 0-2047
const uint16_t LEN_X_MOVING_SPEED= 2; // (XL-320)
const uint16_t ADDR_X_TORQUE_ENABLE= 24; // (XL-320) TORQUE_ENABLE adress 24 0 off 1 on
const uint16_t LEN_X_TORQUE_ENABLE= 1;
const uint8_t CONTROL_MODE = 2; // for XL320 adress 11 byt 1 mode 2 joint
Dynamixel2Arduino dxl_mini(DXL_SERIAL);
ParamForSyncWriteInst_t GP_sync_write_param; // Parameters structure for SW GP packet
ParamForSyncReadInst_t GP_sync_read_param; // Parameters structure for SR GP packet
RecvInfoFromStatusInst_t GP_read_result; // Structure for results of SR PP command
uint8_t dxl_error = 0; // Dynamixel error
int32_t dxl_present_position[GP_DXL_CNT] = { 0 }; // Tableau de position actuelle
int32_t dxl_goal_position[GP_DXL_CNT] = { 0 }; // Tableau de position d’objectif
bool read_present_positions_OK = false;
// Section MOUVEMENT
int _STEP_TIME_0 = millis();
int _STEP_TIME_1 = millis();
int _pose_init[17] ={390, 0, 0, -73.24, 73.24, 0, 0, 0, 0, -26.37, 26.37, 29.3, -29.3, 13.18, -13.18, 0, 0};
int salut_main [6][17] = { {100,-0.29, -150 ,-73.54 ,72.36, -0.29, 0.59 ,-0.29 ,-0.29, -26.66, 26.07 ,29, -29.59, 12.89, -13.48, -0.29, -0.29},
{0,-0.29 ,-149.71, -73.54, 51.27, -0.29, -33.69, -0.29, -0.29 ,-26.66, 26.07, 29 ,-29.59, 12.89, -13.48 ,-0.29 ,-0.29},
{0,-0.29 ,-150, -73.54, 91.7, -0.29, 32.81, -0.29 ,-0.29 ,-26.66 ,26.07 ,29 ,-29.59, 12.89, -13.48, -0.29, -0.29},
{0,-0.29 ,-149.71, -73.54, 51.27, -0.29, -33.69 ,-0.29, -0.29, -26.66 ,26.07 ,29 ,-29.59, 12.89, -13.48, -0.29, -0.29},
{0,-0.29 ,-150, -73.54, 91.7, -0.29, 32.81, -0.29, -0.29, -26.66, 26.07, 29 ,-29.59 ,12.89 ,-13.48, -0.29 ,-0.29},
{0,0 ,0 ,-73.24 ,73.24 ,0, 0, 0, 0 ,-26.37, 26.37, 29.3 ,-29.3 ,13.18 ,-13.18 ,0 ,0 } };
/*int salut_2 [6][17] = { {_STEP_TIME_1,-22.27 ,-44.24, -73.24, 73.24, 0, 0, 0, 0, -26.37, 26.07, 29.3, -29.3, 13.18, -13.48, 0, 0, },
{_STEP_TIME_1,-22.27,-44.24, -62.99, 92.58, -71.78, 58.59, 0, 0, -26.37, 26.07, 29.3, -29.3, 13.18, -13.48, 0, 0 },
{_STEP_TIME_1,-22.27, -69.73, -62.99, 92.58, -71.78, 58.59, 0, 0, -79.39, 82.91, 29.3, -29.3, -10.25, 10.55, 0, 0},
{_STEP_TIME_1,-22.27,-69.73, -62.99, 92.58, -71.78, 58.59, 0, 0, -79.39, 82.91, 29.3, -29.3, -10.25, 10.55, 0, 0},
{_STEP_TIME_1,-22.27, -44.24, -73.24, 73.24, 0, 0, 0, 0, -26.37, 26.07, 29.3, -29.3, 13.18, -13.48, 0, 0},
{_STEP_TIME_1,0, 0, -73.24, 73.24, 0, 0, 0, 0, -26.37, 26.37, 29.3, -29.3, 13.18, -13.18, 0, 0 } }; */
// ********** Function prototypes Prototypes de fonctions
int CalcAngle2Raw(int angle);
//bool check_motion();
int GP_time = _pose_init[0]; //c’est-à-dire _STEP_TIME_0
int CalcAngle2Raw(int angle)
{
return (int)(round((angle * 1023.0 / 300.0 + 512.0)));// return (int)(round((angle * 1023.0 / 300.0 + 512.0)));XL 320
} // fin de CalcAngle2Raw (angle int)
//**** Cet espace de noms est requis pour utiliser les noms d’éléments de la table de contrôle
using namespace ControlTableItem;
void setup()
{
int goal_position_raw = 0; // raw valeur brute de la position de l’objectif DXL
Serial.begin(115200);
while(!Serial); // attendez que l’opérateur ouvre Serial Monitor
//****** Configuration des Dynamixels pour mini ***pour XL320
dxl_mini.begin(1000000);
dxl_mini.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
dxl_mini.scan();
Serial.println("Fin de la numérisation des DXL du robot ");
// Préparation du paquet GP SW
GP_sync_write_param.addr = ADDR_X_GOAL_POSITION;
GP_sync_write_param.length = LEN_X_GOAL_POSITION;
GP_sync_write_param.id_count = GP_DXL_CNT;
for(int i = 0; i < GP_DXL_CNT; i++){
GP_sync_write_param.xel[i].id = GP_DXL_ID_LIST[i];
}
Serial.println(“Fin de la préparation du paquet GP SW”);
// Préparation du paquet GP SR
GP_sync_read_param.addr = ADDR_X_PRESENT_POSITION; // Present Position of DYNAMIXEL-X series
GP_sync_read_param.length = LEN_X_PRESENT_POSITION;
GP_sync_read_param.id_count = GP_DXL_CNT; // Number of servos in Position Control Mode
for(int i = 0; i < GP_DXL_CNT; i++){
GP_sync_read_param.xel[i].id = GP_DXL_ID_LIST[i];
}
Serial.println(“Fin de la préparation du paquet GP SR”);
// Configuration des servos GP dans la zone EEPROM : mode contrôle contrôle de position
// et dans la zone RAM : accélération du profil, vitesse du profil
for(int i = 0; i < GP_DXL_CNT; i++){
dxl_mini.torqueOff(GP_DXL_ID_LIST[i]); // Torque OFF
//dxl_mini.writeControlTableItem( GP_DXL_ID_LIST[i],CONTROL_MODE); //
dxl_mini.setOperatingMode(GP_DXL_ID_LIST[i], OP_POSITION); // Position Control
dxl_mini.torqueOn(GP_DXL_ID_LIST[i]); // Torque ON
delay(100); // attendez un peu avant de travailler dans la zone RAM
}
Serial.println(“Fin de la configuration des servos GP”);
delay(3000);
Serial.println(“Mettre le robot dans la pose initiale”);
// Réglez le robot sur la pose _ via SyncWrite
// Allouez la valeur brute de chaque position d’objectif dans le paquet GP SW, un DXL à la fois en utilisant les valeurs du tableau global _[1] à _[17]
for (int i = 0; i < GP_DXL_CNT; i++) // DXL 1 à 16
{
goal_position_raw = CalcAngle2Raw(_pose_init[i+1]); // convertir l’angle de mouvement en position d’objectif brute [0-4096], _[0] n’est pas utilisé ici
memcpy(GP_sync_write_param.xel[i].data, &goal_position_raw, sizeof(goal_position_raw));
}
dxl_mini.syncWrite(GP_sync_write_param); // send GP SW Packet
delay(3000);
// fin de l’installation() et de la pose initiale"
Serial.println(“jouer mouvement bras”);
for (int frame_no = 0; frame_no < 6; frame_no++) // 4 images de mouvement ont été définies pour le mouvement vers l’avant
{
GP_time = (int)(round(salut_main[frame_no][0] )); //c’est-à-dire STEP TIME 1 modifié avec le facteur de vitesse
//(int)(round(forward[frame_no][0] )); //c’est-à-dire STEP TIME 1 modifié avec le facteur de vitesse
for (int i = 0; i < GP_DXL_CNT; i++) // Définition des valeurs GP brutes pour les Walking DXL 1 à 8
{
goal_position_raw = (CalcAngle2Raw(salut_main[frame_no][i + 1]) ); //convertir l'angle de mouvement avec Adj Offset en Raw GP
memcpy(GP_sync_write_param.xel[i].data, &goal_position_raw, sizeof(goal_position_raw));
}
if (dxl_mini.syncWrite(GP_sync_write_param) == false) // envoyer le paquet GP SW
{
Serial.println("Sfin mouvement");
}
delay(400); // en attendant que chaque image clé soit jouée en ms
} // fin de pour (numéro de trame)
} // fin du mouvement bras
void loop()
// Réinitialisation du robot à la pose d’initialisation
{
int GP_time = _pose_init[0]; //c’est-à-dire _STEP_TIME_0
int goal_position_raw = 0; // valeur brute de la position de l’objectif DXL
for (int i = 0; i < GP_DXL_CNT; i++) // Définir GP pour les DXL 1 à 16
{
goal_position_raw = CalcAngle2Raw(pose_init[i+1]); //convertir l’angle de mouvement en position d’objectif brute [0-1023]
memcpy(GP_sync_write_param.xel[i].data, &goal_position_raw, sizeof(goal_position_raw));
}
if (dxl_mini.syncWrite(GP_sync_write_param) == false) // envoyer le paquet GP SW si faux
{
Serial.println(“Échec du logiciel”); //Échec du logiciel pour SW_TC()
// return 0;
}
delay(3000); // en attente de la réinitialisation de la pose pour jouer en ms
} >`
