DYNAMIXEL Actuator
I an having troubles with the Dynamixel XL430-w250. I am attempting to control the servo with the OpenRB-150 board.
Issue Description
Earlier this week I uploaded the sketch below using the Arduino IDE to the OpenRB-150. When uploading, the upload port could not be found. I then tried to upload a different sketch (the Position mode example from Robotis), which uploaded fine. The problem is that any commands to the servos now won’t work. All values I get from the servos are 0.
Other Details:
- The USB-C serial port is fully functional
- The sketch runs as expected, all except any commands to the Dynamixel Servo
- The Red LED on the servo lights up with the Open-RB board when power is connected
- I have uploaded the usb_to_dynamixel sketch to try the DynamixelWizard, but it cannot detect the Servo. (The COM port is open)
- Prior to uploading the predictive controls sketch I wasn’t having any issues using the OpenRB with the motor
- The Pred. Cont. sketch used ~90% of the program storage space
- For wiring I just have a 12V battery connected the the terminal Vin, connection to the USB-C port, and one Dynamixel connected
Additional Information/Attachments
Predictive Controls Sketch:
#include <DynamixelShield.h>
#include <DYNAMIXEL2Arduino.h>
#include <math.h>
#define DEBUG_SERIAL Serial
#define DXL_SERIAL Serial1
const uint8_t DXL_ID = 2;
const float DXL_PROTOCOL_VERSION = 2.0;
// control table parameters
uint16_t Kp = 630;
uint16_t Ki = 2840;
int i = 0; //For loop counter
int j = 0; //For loop counter
int n = 0; //While loop counter
int flip[2] = {1, -1};
int vel = 0;
long t_start;
long t = 0;
int target = 0; // target angle sent to the motor
int n_inputs = 260;
int repeats = 1;
int Tsamp = 15;
int dt;
bool forced;
const int N = 40;
int P_idx1;
int P_idx2;
const int m = 6;
const int o = 27;
float P[6][27][40] = {{{0, 0, 0, 3.125000e-02, 7.812500e-03, 5.381944e-02, 5.381944e-02, 5.875496e-02, 1.414683e-01, 1.478919e-01, 2.097718e-01, 3.475198e-01, 3.863095e-01, 3.767609e-01, 4.823909e-01, 5.546875e-01, 6.130952e-01, 7.036954e-01, 7.262649e-01, 7.588789e-01, 8.871776e-01, 9.290923e-01, 9.579861e-01, 1.095734e+00, 1.157093e+00, 1.209425e+00, 1.338492e+00, 1.369221e+00, 1.460863e+00, 1.502877e+00, 1.591047e+00, 1.660218e+00, 1.683904e+00, 1.794122e+00, 1.864261e+00, 1.928745e+00, 2.001910e+00, 2.069345e+00, 2.103447e+00, 2.220461e+00},
{0, 0, 0, 1.444659e-02, 8.365508e-03, 4.640350e-02, 8.660126e-02, 1.253100e-01, 1.994262e-01, 2.736391e-01, 3.340442e-01, 4.129168e-01, 4.901366e-01, 5.747166e-01, 6.520115e-01, 7.143303e-01, 7.957042e-01, 8.621579e-01, 9.107814e-01, 9.954145e-01, 1.064692e+00, 1.131540e+00, 1.199408e+00, 1.255068e+00, 1.317614e+00, 1.383559e+00, 1.448014e+00, 1.528782e+00, 1.606891e+00, 1.666641e+00, 1.730638e+00, 1.823247e+00, 1.907600e+00, 1.991183e+00, 2.044934e+00, 2.126260e+00, 2.202209e+00, 2.258798e+00, 2.337533e+00, 2.414687e+00},
{0, 0, 0, 3.090162e-03, 2.415463e-02, 6.568865e-02, 1.195380e-01, 1.781407e-01, 2.443181e-01, 3.291692e-01, 4.128534e-01, 4.916120e-01, 5.748771e-01, 6.609213e-01, 7.319237e-01, 8.090199e-01, 8.786135e-01, 9.442639e-01, 1.015166e+00, 1.084451e+00, 1.151283e+00, 1.214892e+00, 1.281663e+00, 1.353585e+00, 1.420303e+00, 1.495634e+00, 1.564010e+00, 1.632850e+00, 1.712730e+00, 1.789109e+00, 1.866715e+00, 1.942423e+00, 2.014796e+00, 2.100814e+00, 2.172334e+00, 2.248116e+00, 2.331030e+00, 2.407384e+00, 2.484730e+00, 2.560864e+00},
{0, 0, 0, 3.803650e-03, 3.319006e-02, 6.969561e-02, 1.265947e-01, 1.918880e-01, 2.657660e-01, 3.421462e-01, 4.319394e-01, 5.155295e-01, 5.984530e-01, 6.790948e-01, 7.640466e-01, 8.359724e-01, 9.133043e-01, 9.877015e-01, 1.057357e+00, 1.125846e+00, 1.195827e+00, 1.262794e+00, 1.327018e+00, 1.397239e+00, 1.468982e+00, 1.538979e+00, 1.610659e+00, 1.683070e+00, 1.755919e+00, 1.835436e+00, 1.908686e+00, 1.978981e+00, 2.054252e+00, 2.131541e+00, 2.209984e+00, 2.282036e+00, 2.362891e+00, 2.442097e+00, 2.513032e+00, 2.588763e+00},
{0, 0, 0, 5.617223e-03, 3.141255e-02, 7.419252e-02, 1.284623e-01, 1.957204e-01, 2.732955e-01, 3.532038e-01, 4.416107e-01, 5.255249e-01, 6.104901e-01, 6.974929e-01, 7.762514e-01, 8.553761e-01, 9.311476e-01, 1.003441e+00, 1.075913e+00, 1.140254e+00, 1.209483e+00, 1.277869e+00, 1.348606e+00, 1.414669e+00, 1.484342e+00, 1.552764e+00, 1.628876e+00, 1.697260e+00, 1.776424e+00, 1.851083e+00, 1.928018e+00, 2.001641e+00, 2.080030e+00, 2.159729e+00, 2.237364e+00, 2.313343e+00, 2.389092e+00, 2.469225e+00, 2.543716e+00, 2.618254e+00},
{0, 0, 0, 7.638780e-03, 3.971455e-02, 8.171235e-02, 1.392394e-01, 2.060275e-01, 2.839996e-01, 3.665439e-01, 4.537059e-01, 5.390505e-01, 6.247158e-01, 7.096680e-01, 7.882674e-01, 8.674095e-01, 9.426341e-01, 1.018294e+00, 1.085931e+00, 1.154659e+00, 1.223490e+00, 1.293423e+00, 1.363472e+00, 1.434660e+00, 1.501942e+00, 1.572713e+00, 1.644549e+00, 1.718814e+00, 1.796638e+00, 1.871274e+00, 1.946790e+00, 2.020209e+00, 2.101349e+00, 2.178934e+00, 2.256056e+00, 2.331934e+00, 2.410237e+00, 2.487938e+00, 2.564651e+00, 2.640023e+00},
{0, 0, 0, 8.240278e-03, 3.682594e-02, 8.245817e-02, 1.366409e-01, 2.055378e-01, 2.796383e-01, 3.612126e-01, 4.469310e-01, 5.329217e-01, 6.179261e-01, 7.051490e-01, 7.839166e-01, 8.652012e-01, 9.383864e-01, 1.014460e+00, 1.085267e+00, 1.157203e+00, 1.225058e+00, 1.295058e+00, 1.363430e+00, 1.433828e+00, 1.505398e+00, 1.577594e+00, 1.650846e+00, 1.725795e+00, 1.797910e+00, 1.874639e+00, 1.949996e+00, 2.026805e+00, 2.104799e+00, 2.183323e+00, 2.259631e+00, 2.334504e+00, 2.412464e+00, 2.486550e+00, 2.566931e+00, 2.643588e+00},
{0, 0, 0, 9.305857e-03, 3.711317e-02, 8.294212e-02, 1.374389e-01, 2.049567e-01, 2.809073e-01, 3.626250e-01, 4.488224e-01, 5.347619e-01, 6.226563e-01, 7.061016e-01, 7.875864e-01, 8.675192e-01, 9.475178e-01, 1.020317e+00, 1.092158e+00, 1.164381e+00, 1.233483e+00, 1.303883e+00, 1.372867e+00, 1.441132e+00, 1.512186e+00, 1.583772e+00, 1.654672e+00, 1.729014e+00, 1.803963e+00, 1.879730e+00, 1.956549e+00, 2.033336e+00, 2.110795e+00, 2.188167e+00, 2.265554e+00, 2.342622e+00, 2.418608e+00, 2.495704e+00, 2.574598e+00, 2.651716e+00},
{0, 0, 0, 1.218372e-02, 4.057077e-02, 8.425707e-02, 1.408437e-01, 2.077156e-01, 2.830234e-01, 3.664864e-01, 4.533250e-01, 5.401888e-01, 6.268400e-01, 7.127284e-01, 7.965105e-01, 8.761843e-01, 9.539270e-01, 1.029183e+00, 1.100104e+00, 1.169452e+00, 1.240091e+00, 1.309051e+00, 1.379014e+00, 1.448398e+00, 1.519965e+00, 1.587949e+00, 1.661643e+00, 1.733831e+00, 1.808628e+00, 1.884596e+00, 1.959916e+00, 2.038044e+00, 2.114689e+00, 2.193107e+00, 2.270077e+00, 2.349729e+00, 2.424823e+00, 2.503048e+00, 2.580730e+00, 2.657661e+00},
{0, 0, 0, 9.377335e-03, 3.885825e-02, 8.164378e-02, 1.382532e-01, 2.076735e-01, 2.818224e-01, 3.651163e-01, 4.492739e-01, 5.364389e-01, 6.229071e-01, 7.075633e-01, 7.908675e-01, 8.722526e-01, 9.483527e-01, 1.025864e+00, 1.100119e+00, 1.171599e+00, 1.242465e+00, 1.313264e+00, 1.381592e+00, 1.452942e+00, 1.525193e+00, 1.596643e+00, 1.669153e+00, 1.742970e+00, 1.818260e+00, 1.893517e+00, 1.966779e+00, 2.043493e+00, 2.120124e+00, 2.196542e+00, 2.274864e+00, 2.350042e+00, 2.428710e+00, 2.507565e+00, 2.583753e+00, 2.661286e+00},
{0, 0, 0, 1.191963e-02, 4.290852e-02, 8.758157e-02, 1.448775e-01, 2.118746e-01, 2.873112e-01, 3.687818e-01, 4.545079e-01, 5.414666e-01, 6.262867e-01, 7.127542e-01, 7.960159e-01, 8.765222e-01, 9.558974e-01, 1.032272e+00, 1.105838e+00, 1.178095e+00, 1.249369e+00, 1.322013e+00, 1.391751e+00, 1.462546e+00, 1.532711e+00, 1.605794e+00, 1.678794e+00, 1.753317e+00, 1.827326e+00, 1.903268e+00, 1.978453e+00, 2.055037e+00, 2.133515e+00, 2.210031e+00, 2.287882e+00, 2.364630e+00, 2.442919e+00, 2.519155e+00, 2.596659e+00, 2.673172e+00},
{0, 0, 0, 1.394369e-02, 4.118744e-02, 8.368319e-02, 1.411861e-01, 2.061843e-01, 2.821761e-01, 3.638079e-01, 4.501329e-01, 5.360395e-01, 6.231278e-01, 7.092577e-01, 7.934198e-01, 8.764032e-01, 9.566119e-01, 1.035136e+00, 1.109526e+00, 1.182259e+00, 1.253683e+00, 1.323691e+00, 1.393208e+00, 1.462870e+00, 1.533541e+00, 1.604528e+00, 1.677255e+00, 1.751356e+00, 1.824746e+00, 1.899966e+00, 1.977426e+00, 2.053814e+00, 2.132812e+00, 2.210311e+00, 2.288700e+00, 2.367800e+00, 2.444853e+00, 2.522654e+00, 2.599461e+00, 2.675589e+00},
{0, 0, 0, 1.165204e-02, 4.014913e-02, 8.286536e-02, 1.378682e-01, 2.038901e-01, 2.780235e-01, 3.588767e-01, 4.432148e-01, 5.309560e-01, 6.189052e-01, 7.046862e-01, 7.892911e-01, 8.694748e-01, 9.480220e-01, 1.025079e+00, 1.100299e+00, 1.173827e+00, 1.245029e+00, 1.317731e+00, 1.387924e+00, 1.459009e+00, 1.531153e+00, 1.603626e+00, 1.676737e+00, 1.750714e+00, 1.826702e+00, 1.901070e+00, 1.977569e+00, 2.054628e+00, 2.132722e+00, 2.208085e+00, 2.286037e+00, 2.362465e+00, 2.439166e+00, 2.516279e+00, 2.592106e+00, 2.670614e+00},
{0, 0, 0, 1.173326e-02, 4.000608e-02, 8.325385e-02, 1.376756e-01, 2.030820e-01, 2.776280e-01, 3.560820e-01, 4.413051e-01, 5.282429e-01, 6.157488e-01, 7.030236e-01, 7.880225e-01, 8.712832e-01, 9.501200e-01, 1.027854e+00, 1.105007e+00, 1.178616e+00, 1.251276e+00, 1.321633e+00, 1.393575e+00, 1.464175e+00, 1.537983e+00, 1.609747e+00, 1.682191e+00, 1.757237e+00, 1.832060e+00, 1.907797e+00, 1.984217e+00, 2.059947e+00, 2.137293e+00, 2.214020e+00, 2.292281e+00, 2.368638e+00, 2.446111e+00, 2.522854e+00, 2.599559e+00, 2.675908e+00},
{0, 0, 0, 1.159184e-02, 3.977658e-02, 8.209034e-02, 1.358776e-01, 2.000111e-01, 2.734314e-01, 3.521794e-01, 4.355549e-01, 5.223661e-01, 6.086444e-01, 6.941580e-01, 7.791433e-01, 8.618208e-01, 9.432858e-01, 1.021181e+00, 1.098906e+00, 1.174734e+00, 1.247974e+00, 1.321017e+00, 1.393333e+00, 1.466321e+00, 1.538939e+00, 1.611616e+00, 1.684987e+00, 1.757742e+00, 1.830566e+00, 1.906275e+00, 1.980959e+00, 2.056304e+00, 2.134026e+00, 2.210572e+00, 2.289460e+00, 2.367306e+00, 2.445283e+00, 2.523147e+00, 2.599384e+00, 2.677944e+00},
{0, 0, 0, 1.182686e-02, 3.930709e-02, 8.096531e-02, 1.338015e-01, 1.969755e-01, 2.701427e-01, 3.498268e-01, 4.320380e-01, 5.182204e-01, 6.037323e-01, 6.895952e-01, 7.741700e-01, 8.566626e-01, 9.374685e-01, 1.016142e+00, 1.093481e+00, 1.169087e+00, 1.245285e+00, 1.318579e+00, 1.390918e+00, 1.463607e+00, 1.535943e+00, 1.609101e+00, 1.682283e+00, 1.755614e+00, 1.829957e+00, 1.904809e+00, 1.981794e+00, 2.057402e+00, 2.134573e+00, 2.211714e+00, 2.289050e+00, 2.366031e+00, 2.444085e+00, 2.520390e+00, 2.596081e+00, 2.674126e+00},
{0, 0, 0, 1.108733e-02, 3.907482e-02, 7.973716e-02, 1.294932e-01, 1.902924e-01, 2.602075e-01, 3.347639e-01, 4.160807e-01, 4.986280e-01, 5.836340e-01, 6.698874e-01, 7.547589e-01, 8.388150e-01, 9.202657e-01, 9.995008e-01, 1.077588e+00, 1.153379e+00, 1.228390e+00, 1.301815e+00, 1.375440e+00, 1.448694e+00, 1.522252e+00, 1.595337e+00, 1.668929e+00, 1.742515e+00, 1.817032e+00, 1.893145e+00, 1.968278e+00, 2.044805e+00, 2.120290e+00, 2.196541e+00, 2.273454e+00, 2.350738e+00, 2.428438e+00, 2.505959e+00, 2.583780e+00, 2.661274e+00},
{0, 0, 0, 1.090021e-02, 3.619033e-02, 7.417775e-02, 1.225785e-01, 1.819051e-01, 2.465117e-01, 3.196489e-01, 3.970629e-01, 4.776835e-01, 5.609838e-01, 6.440912e-01, 7.272546e-01, 8.090179e-01, 8.909574e-01, 9.700648e-01, 1.048870e+00, 1.124746e+00, 1.200858e+00, 1.276735e+00, 1.350870e+00, 1.425229e+00, 1.498319e+00, 1.572168e+00, 1.646564e+00, 1.720969e+00, 1.796423e+00, 1.870674e+00, 1.946699e+00, 2.022858e+00, 2.099301e+00, 2.175163e+00, 2.251816e+00, 2.328868e+00, 2.404763e+00, 2.483576e+00, 2.560518e+00, 2.638158e+00},
{0, 0, 0, 1.126315e-02, 3.588046e-02, 7.172261e-02, 1.182689e-01, 1.721997e-01, 2.351435e-01, 3.036599e-01, 3.775983e-01, 4.565262e-01, 5.374162e-01, 6.181566e-01, 7.011226e-01, 7.823695e-01, 8.625526e-01, 9.414581e-01, 1.020331e+00, 1.097544e+00, 1.174019e+00, 1.249594e+00, 1.324315e+00, 1.398052e+00, 1.472214e+00, 1.546527e+00, 1.619427e+00, 1.694582e+00, 1.770153e+00, 1.844888e+00, 1.920562e+00, 1.995941e+00, 2.072203e+00, 2.148155e+00, 2.225379e+00, 2.302203e+00, 2.378769e+00, 2.455005e+00, 2.532238e+00, 2.609848e+00},
{0, 0, 0, 1.119936e-02, 3.383543e-02, 6.797504e-02, 1.113113e-01, 1.635610e-01, 2.221748e-01, 2.876491e-01, 3.595004e-01, 4.346679e-01, 5.121062e-01, 5.903073e-01, 6.710042e-01, 7.502724e-01, 8.297107e-01, 9.102621e-01, 9.893529e-01, 1.067254e+00, 1.144471e+00, 1.220099e+00, 1.295476e+00, 1.370335e+00, 1.444154e+00, 1.518486e+00, 1.593487e+00, 1.668108e+00, 1.742816e+00, 1.818864e+00, 1.894247e+00, 1.970281e+00, 2.045934e+00, 2.121855e+00, 2.198840e+00, 2.275419e+00, 2.351325e+00, 2.428957e+00, 2.506058e+00, 2.584334e+00},
{0, 0, 0, 1.038397e-02, 3.327013e-02, 6.566612e-02, 1.073245e-01, 1.573602e-01, 2.128958e-01, 2.751943e-01, 3.431295e-01, 4.144158e-01, 4.888352e-01, 5.658424e-01, 6.438721e-01, 7.234983e-01, 8.025473e-01, 8.831546e-01, 9.611915e-01, 1.039333e+00, 1.116223e+00, 1.192461e+00, 1.267441e+00, 1.342825e+00, 1.418671e+00, 1.492511e+00, 1.568398e+00, 1.643380e+00, 1.718010e+00, 1.793542e+00, 1.869811e+00, 1.945416e+00, 2.021165e+00, 2.096926e+00, 2.172802e+00, 2.249803e+00, 2.326921e+00, 2.404186e+00, 2.481832e+00, 2.558797e+00},
{0, 0, 0, 9.770557e-03, 3.134358e-02, 6.291405e-02, 1.030395e-01, 1.496163e-01, 2.034727e-01, 2.628851e-01, 3.268836e-01, 3.959903e-01, 4.678199e-01, 5.420258e-01, 6.182430e-01, 6.951532e-01, 7.737579e-01, 8.528041e-01, 9.311345e-01, 1.010064e+00, 1.087323e+00, 1.164014e+00, 1.239609e+00, 1.316028e+00, 1.391497e+00, 1.466637e+00, 1.542470e+00, 1.616249e+00, 1.691126e+00, 1.767555e+00, 1.843506e+00, 1.918611e+00, 1.994567e+00, 2.072194e+00, 2.147571e+00, 2.223610e+00, 2.301605e+00, 2.378120e+00, 2.455148e+00, 2.531930e+00},
{0, 0, 0, 9.773318e-03, 3.024679e-02, 5.936943e-02, 9.825362e-02, 1.423627e-01, 1.943273e-01, 2.496967e-01, 3.105017e-01, 3.761182e-01, 4.451485e-01, 5.176808e-01, 5.921164e-01, 6.674927e-01, 7.439833e-01, 8.217416e-01, 8.996121e-01, 9.766648e-01, 1.053916e+00, 1.131314e+00, 1.207916e+00, 1.285289e+00, 1.361324e+00, 1.437114e+00, 1.513588e+00, 1.588156e+00, 1.663690e+00, 1.738136e+00, 1.814291e+00, 1.890309e+00, 1.967036e+00, 2.042915e+00, 2.119382e+00, 2.195838e+00, 2.272103e+00, 2.348927e+00, 2.425190e+00, 2.502585e+00},
{0, 0, 0, 9.028726e-03, 2.872910e-02, 5.775682e-02, 9.309158e-02, 1.362694e-01, 1.849931e-01, 2.389236e-01, 2.979707e-01, 3.607236e-01, 4.261845e-01, 4.944865e-01, 5.663608e-01, 6.390160e-01, 7.141404e-01, 7.895840e-01, 8.673995e-01, 9.442547e-01, 1.021328e+00, 1.098819e+00, 1.175550e+00, 1.252302e+00, 1.327672e+00, 1.403627e+00, 1.479836e+00, 1.556044e+00, 1.631249e+00, 1.707025e+00, 1.783334e+00, 1.857771e+00, 1.934511e+00, 2.010836e+00, 2.087244e+00, 2.165290e+00, 2.241034e+00, 2.317571e+00, 2.394304e+00, 2.472443e+00},
{0, 0, 0, 8.342259e-03, 2.741848e-02, 5.544084e-02, 9.026109e-02, 1.310026e-01, 1.779745e-01, 2.300690e-01, 2.853996e-01, 3.443952e-01, 4.070352e-01, 4.730213e-01, 5.409583e-01, 6.118194e-01, 6.858237e-01, 7.600944e-01, 8.357350e-01, 9.109699e-01, 9.873551e-01, 1.062234e+00, 1.139879e+00, 1.216333e+00, 1.294649e+00, 1.371098e+00, 1.448291e+00, 1.523092e+00, 1.598730e+00, 1.674361e+00, 1.750106e+00, 1.826559e+00, 1.903315e+00, 1.979593e+00, 2.055719e+00, 2.132199e+00, 2.208873e+00, 2.285350e+00, 2.362017e+00, 2.439203e+00},
{0, 0, 0, 8.718903e-03, 2.642999e-02, 5.253637e-02, 8.593392e-02, 1.244621e-01, 1.698800e-01, 2.190068e-01, 2.726380e-01, 3.289387e-01, 3.898636e-01, 4.531757e-01, 5.196015e-01, 5.869262e-01, 6.575572e-01, 7.283970e-01, 8.024502e-01, 8.766428e-01, 9.514524e-01, 1.027265e+00, 1.103085e+00, 1.179298e+00, 1.256029e+00, 1.332242e+00, 1.407943e+00, 1.485235e+00, 1.561748e+00, 1.639027e+00, 1.714650e+00, 1.791934e+00, 1.867445e+00, 1.944743e+00, 2.022351e+00, 2.098928e+00, 2.175608e+00, 2.252716e+00, 2.329028e+00, 2.405836e+00},
{0, 0, 0, 8.280643e-03, 2.636361e-02, 5.141812e-02, 8.323044e-02, 1.222323e-01, 1.664677e-01, 2.146190e-01, 2.669120e-01, 3.242206e-01, 3.817497e-01, 4.447234e-01, 5.092095e-01, 5.763282e-01, 6.447439e-01, 7.144721e-01, 7.850658e-01, 8.580554e-01, 9.325669e-01, 1.008175e+00, 1.083127e+00, 1.158725e+00, 1.235633e+00, 1.311455e+00, 1.389886e+00, 1.466363e+00, 1.542833e+00, 1.620386e+00, 1.697295e+00, 1.773525e+00, 1.851300e+00, 1.928220e+00, 2.006419e+00, 2.082249e+00, 2.158704e+00, 2.235630e+00, 2.311881e+00, 2.389660e+00}},
{{0, 0, 0, 7.153770e-01, 1.402629e+00, 1.831845e+00, 2.139435e+00, 2.285094e+00, 2.373363e+00, 2.382044e+00, 2.374231e+00, 2.389856e+00, 2.396801e+00, 2.393229e+00, 2.437227e+00, 2.475769e+00, 2.554390e+00, 2.606721e+00, 2.710863e+00, 2.871776e+00, 3.033556e+00, 3.218527e+00, 3.284127e+00, 3.495139e+00, 3.619593e+00, 3.706895e+00, 3.844742e+00, 3.918502e+00, 4.064311e+00, 4.127059e+00, 4.284772e+00, 4.325918e+00, 4.418527e+00, 4.521553e+00, 4.566939e+00, 4.674653e+00, 4.778199e+00, 4.854315e+00, 4.933904e+00, 5.042733e+00},
{0, 0, 0, 3.892671e-01, 7.020974e-01, 9.557546e-01, 1.140924e+00, 1.270046e+00, 1.340455e+00, 1.393477e+00, 1.425542e+00, 1.446253e+00, 1.495557e+00, 1.526477e+00, 1.563788e+00, 1.623087e+00, 1.705669e+00, 1.795888e+00, 1.877153e+00, 1.991501e+00, 2.105079e+00, 2.209787e+00, 2.321461e+00, 2.409256e+00, 2.522622e+00, 2.633808e+00, 2.721237e+00, 2.804199e+00, 2.904376e+00, 2.978177e+00, 3.039936e+00, 3.134336e+00, 3.202764e+00, 3.285315e+00, 3.375497e+00, 3.443406e+00, 3.517456e+00, 3.597378e+00, 3.680360e+00, 3.774453e+00},
{0, 0, 0, 1.933668e-01, 3.525932e-01, 4.863602e-01, 6.006789e-01, 6.992656e-01, 7.749704e-01, 8.428006e-01, 9.014342e-01, 9.634240e-01, 1.020142e+00, 1.088368e+00, 1.145095e+00, 1.214081e+00, 1.297658e+00, 1.375123e+00, 1.458278e+00, 1.547181e+00, 1.632074e+00, 1.721995e+00, 1.808330e+00, 1.894505e+00, 1.991221e+00, 2.069471e+00, 2.160743e+00, 2.248423e+00, 2.329905e+00, 2.408180e+00, 2.483030e+00, 2.564378e+00, 2.646865e+00, 2.723321e+00, 2.792649e+00, 2.871610e+00, 2.953260e+00, 3.035782e+00, 3.111594e+00, 3.194030e+00},
{0, 0, 0, 1.236773e-01, 2.356584e-01, 3.413409e-01, 4.381865e-01, 5.229209e-01, 6.028935e-01, 6.787620e-01, 7.508879e-01, 8.195557e-01, 8.906043e-01, 9.603053e-01, 1.035413e+00, 1.106913e+00, 1.186372e+00, 1.262011e+00, 1.343016e+00, 1.423996e+00, 1.503495e+00, 1.587023e+00, 1.665075e+00, 1.749828e+00, 1.830301e+00, 1.914965e+00, 1.994661e+00, 2.080346e+00, 2.160061e+00, 2.241031e+00, 2.318348e+00, 2.399222e+00, 2.475326e+00, 2.555972e+00, 2.636307e+00, 2.713918e+00, 2.794409e+00, 2.871735e+00, 2.949519e+00, 3.028260e+00},
{0, 0, 0, 9.791883e-02, 1.891034e-01, 2.750434e-01, 3.572941e-01, 4.403175e-01, 5.191998e-01, 5.963170e-01, 6.714733e-01, 7.464853e-01, 8.215473e-01, 8.977689e-01, 9.738485e-01, 1.047755e+00, 1.127894e+00, 1.206622e+00, 1.284196e+00, 1.362692e+00, 1.440585e+00, 1.523425e+00, 1.604483e+00, 1.684588e+00, 1.763086e+00, 1.842678e+00, 1.922349e+00, 2.001044e+00, 2.080696e+00, 2.159639e+00, 2.237792e+00, 2.316163e+00, 2.392291e+00, 2.470816e+00, 2.554546e+00, 2.628577e+00, 2.707500e+00, 2.788522e+00, 2.864568e+00, 2.941109e+00},
{0, 0, 0, 7.894065e-02, 1.576581e-01, 2.368093e-01, 3.145699e-01, 3.939505e-01, 4.710209e-01, 5.505171e-01, 6.285450e-01, 7.072110e-01, 7.872068e-01, 8.635324e-01, 9.425126e-01, 1.019632e+00, 1.097285e+00, 1.175072e+00, 1.254302e+00, 1.332241e+00, 1.407262e+00, 1.485258e+00, 1.564969e+00, 1.641568e+00, 1.719560e+00, 1.799347e+00, 1.877333e+00, 1.954738e+00, 2.034034e+00, 2.111872e+00, 2.189332e+00, 2.267307e+00, 2.344834e+00, 2.423721e+00, 2.499061e+00, 2.576266e+00, 2.653425e+00, 2.735176e+00, 2.813586e+00, 2.890909e+00},
{0, 0, 0, 6.722748e-02, 1.358872e-01, 2.081328e-01, 2.821441e-01, 3.594672e-01, 4.368451e-01, 5.173140e-01, 5.977904e-01, 6.780563e-01, 7.566562e-01, 8.325259e-01, 9.132422e-01, 9.914711e-01, 1.070537e+00, 1.150078e+00, 1.225136e+00, 1.302199e+00, 1.380582e+00, 1.458616e+00, 1.534921e+00, 1.610746e+00, 1.688493e+00, 1.765028e+00, 1.844186e+00, 1.921681e+00, 1.999738e+00, 2.078537e+00, 2.154011e+00, 2.231030e+00, 2.309085e+00, 2.386980e+00, 2.466784e+00, 2.543513e+00, 2.621919e+00, 2.697206e+00, 2.775525e+00, 2.857240e+00},
{0, 0, 0, 6.061081e-02, 1.242923e-01, 1.924010e-01, 2.630451e-01, 3.385178e-01, 4.158878e-01, 4.966158e-01, 5.762526e-01, 6.556492e-01, 7.364144e-01, 8.186654e-01, 8.964065e-01, 9.777018e-01, 1.055573e+00, 1.134116e+00, 1.209863e+00, 1.288567e+00, 1.366902e+00, 1.442085e+00, 1.515854e+00, 1.594519e+00, 1.670421e+00, 1.746480e+00, 1.824089e+00, 1.900003e+00, 1.976037e+00, 2.053942e+00, 2.132485e+00, 2.212246e+00, 2.288268e+00, 2.367771e+00, 2.444260e+00, 2.524256e+00, 2.601463e+00, 2.678837e+00, 2.757518e+00, 2.834532e+00},
//SEE THE REST OF THE MATRIX BELOW IN THE COMMENTS
DynamixelShield dxl(DXL_SERIAL); // Declaring dynam. shield handle
//This namespace is required to use Control table item names
using namespace ControlTableItem;
int CoolDown() {
while (1){
if (dxl.readControlTableItem(PRESENT_TEMPERATURE, DXL_ID) > 35){
delay(60*1000);
}
else{
break;
}
}
}
// function waits until the motor has stopped
int wait_stop(){
dxl.setGoalVelocity(DXL_ID, 0);
n = 0;
vel = dxl.getPresentVelocity(DXL_ID);
while (vel != 0 && n <= 50){
if (vel == dxl.getPresentVelocity(DXL_ID)){ // motor isnt moving
n = n + 1; // condition is required in case of a steady state error
}
else{ // reset the counter if the motor moved
n = 0;
}
vel = dxl.getPresentVelocity(DXL_ID); // get current position
}
delay(100); // gives the motor some time to rest
}
void setup() {
DEBUG_SERIAL.begin(115200); // Open Serial connection
while(!DEBUG_SERIAL); //Wait until the serial port is opened
// Setting up the motor
dxl.begin(1000000);
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
dxl.ping(DXL_ID);
// Turn off torque when configuring items in EEPROM area (From shield example code)
dxl.torqueOff(DXL_ID);
dxl.setOperatingMode(DXL_ID, OP_EXTENDED_POSITION);
dxl.torqueOn(DXL_ID);
dxl.setGoalPosition(DXL_ID, 0); // Go to home position
delay(1000);
dxl.torqueOff(DXL_ID);
dxl.setOperatingMode(DXL_ID, OP_VELOCITY);
dxl.writeControlTableItem(RETURN_DELAY_TIME, DXL_ID, 25);
dxl.torqueOn(DXL_ID);
// Velocity control gains
dxl.writeControlTableItem(VELOCITY_P_GAIN, DXL_ID, Kp);
dxl.writeControlTableItem(VELOCITY_I_GAIN, DXL_ID, Ki);
}
void loop() {
n_inputs = 1;
Tsamp = 5;
t = 0;
// int R[15] = {50, 150, 180, 250, 10, -80, -220, -160, -50, 50, 60, 55, 59, 64, 61};
int R = 400;
int measurments = 40;
// Intializing SPC Variables
int t_meas[40];
int t_calc[40];
int y;
int e[40];
float y_pred[N] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
float dy_pred[N] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int du = 0;
int du_arr[40];
float pred_arr[40];
int u = 0;
int u_1 = 0;
int u_arr[40];
float phi;
float phi_arr[40];
float lambda = 10;
float d_lambda = 0.5;
int u_max = 260;
int u_min = -260;
int P_idx1_arr[40];
int P_idx2_arr[40];
DEBUG_SERIAL.println(0);
t_start = millis();
for (j = 0; j<n_inputs; j++){ // for 200 inputs
target = R;
for (n = 1; n < measurments; n++){ //collect n data points
t_meas[n] = millis() - t_start;
// Measure current Speed
y = dxl.getPresentPosition(DXL_ID);
vel = dxl.getPresentVelocity(DXL_ID);
P_idx1 = abs(floor((vel)/50));
if (P_idx1 < 0){
P_idx1 = 0;
}
else if (P_idx1 >= m){
P_idx1 = m-1;
}
P_idx1_arr[n] = P_idx1;
P_idx2 = abs(floor((u_1)/10));
if (P_idx2 < 0){
P_idx2 = 0;
}
else if (P_idx2 >= o){
P_idx2 = o-1;
}
P_idx2_arr[n] = P_idx2;
phi = y - y_pred[0];
phi_arr[n] = phi;
// Adjust the damping coeffcient accordingly
if (phi > 0){
lambda-= d_lambda;
}
else{
lambda+= d_lambda;
}
// Update predictions
for (i = 1; i < N; i++) {
y_pred[i-1] = y_pred[i] + phi;
}
y_pred[N-1] = 2* y_pred[N-2] + phi - y_pred[N-3];
// Determine du
e[n] = R-y;
du = (R - y_pred[3])/P[P_idx1][P_idx2][3]/lambda;
u = u_1 + du;
forced = false;
if (u > u_max){
u = u_max;
}
else if(u<u_min){
u = u_min;
}
du = u - u_1;
if (du > 50){
du = 50;
}
else if(u < -50){
du = -50;
}
u = u_1 + du;
du_arr[n] = du;
u_arr[n] = u;
// Updating the Prediction
for (i = 0; i < N; i++) {
dy_pred[i] = P[P_idx1][P_idx2][i]*du;
}
for (i = 0; i < N; i++){
y_pred[i]+= dy_pred[i];
}
pred_arr[n] = y_pred[3];
t_calc[n] = millis() - t_meas[n] - t_start;
while ((millis() - t_meas[n] - t_start) < Tsamp){}
dxl.setGoalVelocity(DXL_ID, u);
u_1 = u;
}
for (n = 1; n < measurments; n++){
DEBUG_SERIAL.print(t_meas[n]);
DEBUG_SERIAL.print(" ");
DEBUG_SERIAL.print(t_calc[n]);
DEBUG_SERIAL.print(" ");
DEBUG_SERIAL.print(u_arr[n]);
DEBUG_SERIAL.print(" ");
DEBUG_SERIAL.print(du_arr[n]);
DEBUG_SERIAL.print(" ");
DEBUG_SERIAL.print(R);
DEBUG_SERIAL.print(" ");
DEBUG_SERIAL.print(e[n]);
DEBUG_SERIAL.print(" ");
DEBUG_SERIAL.print(phi_arr[n]);
DEBUG_SERIAL.print(" ");
DEBUG_SERIAL.println(pred_arr[n]);
}
if (dxl.readControlTableItem(PRESENT_TEMPERATURE, DXL_ID) > 50){
CoolDown();
}
}
// signal the end of the test
DEBUG_SERIAL.println("exit");
dxl.setGoalVelocity(DXL_ID, 0);
delay(100);
exit(0);
}
Note:
The large P matrix is a discrete, normalized model for the position of the Dynamixel motor for changes in the goal velocity input. (How will the position change in “t” msec if the goal velocity is changed.) The dimensions are P[starting position][current velocity][time]. There are 6 starting position segments (0, 50 …250), 27 current velocity segements (0, 10, 20… 250) and the time is from 0 to 200ms in jumps of 5ms.
Turns out the matrix is too large to include here, so I will add it below in the comments.
I am not very experienced in electronics, if anyone has any idea what the problem may be I would really appreciate the input. Thank you.