Downloading from GitHub is advised for actually running the robot, to ensure you have the most recent version.
Presently, control signals are provided by an external laptop through a U2D2 serial converter (Robotis, Seoul, South Korea) connected to the power board. The laptop runs a MATLAB script that writes servo angle commands formulated by the kinematic solver to each servo. The script then reads the present servo angles and the current draw of each servo. The MATLAB script additionally communicates over Serial communication with the OpenCM microcontroller on the robot running an Arduino (Arduino, Monza, Italy) script that collects and broadcasts strain data from the 36 strain gauge locations. The read-write loop runs between 35-40 Hz.
The following sections will describe each component of this controller architecture in greater detail.
Arduino Script
Full Arduino script
The Arduino script runs on the robot's OpenCM microcontroller and facilitates the collection of the strain data by calibrating the gauges at the start of the trial, organizing the switching of the deMUX board during data collection, and sending the strain data over Serial communication to the MATLAB program.
If strain data is not being collected for a trial, using this script is unnecessary and can be omitted.
Upon startup, the program sets all the digital pins on the microcontroller LOW and specifies them as outputs. It similarly specifies all of the analog pins as inputs, then waits for the MATLAB program to send a command over Serial to continue the program. Once this command is received, the program calibrates the rest point of all 36 strain gauges (lines 62-102) following the following process:
The digital pins for a particular leg and side of the amplifier board are set HIGH to direct the I2C connections to the desired strain circuit. The digital pin for a particular strain location is also set HIGH to connect that strain circuit to that leg's analog pin.
A strain reading is collected from that location and compared to a user-defined target rest value.
IF the strain is not within a user-defined threshold of the target, THEN the wiper value of the circuit's digipot is changed.
IF the value is less than the lower calibration limit, THEN the wiper value is incremented.
IF the value is greater than the upper calibration limit, THEN the wiper value is decremented
The strain is rerecorded to check the wiper value's effect on the reading.
This process is repeated until the strain reading is within the calibration range, then repeated for all strain gauge locations across all legs.
Once calibration is complete, the program enters the main strain reading loop (lines 104-138). The loop requires the MATLAB program to send a command variable requesting strain data to initiate strain reading. Otherwise, the loop does nothing. When active, the loop continuously reads strain data from each location on each leg. It accomplishes this by cycling through setting one of the six digital command pins HIGH, thereby connecting a particular location to its leg's analog pin, then reading from all six legs' pins. The program then sends the combined data from all 36 locations (allSGValues on line 128) as one line over Serial to the MATLAB program before waiting for 2 ms and continuing with another set of strain readings. In this way, the Arduino program facilitates sending calibrated strain data to the MATLAB program when requested.
MATLAB Script
Full MATLAB script
The current function of the MATLAB script is to control the robot using feedforward kinematic replay from the solver, then collect and record sensory data from the servos and the strain gauges. It does so by first importing DoF angle and torque data from the output of an instance of the solver defined by the user (lines 87-96). This data is then preprocessed before the trial begins (lines 100-189). To account for differences in component rigidity between the solver and the physical platform (e.g., gear backlash, 3-D printed material elasticity, foot slippage) causing unwanted deflections and sagging, a bias term is added to the solver's angle commands equal to the solver's expected actuator torque divided by the stiffness coefficient of the actuator (i.e., the proportional feedback gain scaled to Nm/rad, kP; line 127). These changes bring the posture of the robot closer to the planned posture in the solver. The biased DoF angle data is then converted from radians to bits to be suitable for sending as actuator commands (lines 131-139). Once the angle data is biased and converted, the program determines at what timestep all legs are in stance (lines 114-123) and shifts the data by that amount (149-150). This shift ensures that all legs begin on the ground for each walking trial. Finally, the data is down-sampled to ensure the correct amount of points for the robot's read-write frequency (lines 144-146). Based on the user-defined number of steps for the trial and the length of the downsampled step data, a numIters variable is defined as the total number of timesteps for the trial (line 152).
Once this pre-processing is completed, the robot control portion of the program begins. Connection to each of the robot's actuators is established and the desired internal parameters (maximum velocity, KP value, etc.) are set in each servomotor. Each actuator is then commanded to the first position in the walking command data. If data is to be recorded from the strain gauges, the program sends a signal over Serial to the OpenCM to start sensor calibration following a button-press. Once it receives a completion indicator from the OpenCM, another button-press has the program command the OpenCM to stream 20 iteration's worth of strain data. This data is later averaged to find the baseline value of each gauge and normalize the strain data post hoc. One last button-press halts the Arduino streaming over Serial and begins the stepping control loop.
The stepping control loop cycles numIters times, each time first commanding the servos to the appropriate angle in the step cycle. The code then continuously reads the actual position and load until the loop time reaches the designated duration actTimeStep specified during the downsampling in order to give the servos time to move. If strain is being recorded, the strain data is additionally commanded from the OpenCM and recorded during this waiting period. Once the loop has executed numIters times, the robot stops walking and the trial ends. A button-press disables the torques for each servo, and the code post-processes the data before saving it to a user-defined location. This post-processing primarily entails converting the angles and loads from bit values into radian and Newton-meter values, respectively, as well as parsing the strings of strain data into a matrix form. The output from each trial is a .mat file with the raw and post-processed data, as well as a text file documenting the details of the trial (number of steps, terrain, step period, etc.).
#include <Wire.h>
float targetStrainBaseline = 300;
float strainBaselineErrorMargin = 20;
int digiPotIDs[3] = {44,46,45}; //The three unique digipot IDs that each amplifier board uses
int numDigiPotIDs = 3;
int analogPorts[6] = {0, 1, 2, 3, 4, 5}; //The ports that will recieve the strain data from the amplifier boards. Each leg get a dedicated port.
int numAnalogPorts = 6;
int MUXPins[6] = {10, 11, 12, 13, 14, 15}; //The digital pins that control how the strain data is routed on the MUX
int MUXPinNum = 6;
int ampMUXPins[2] = {22,23}; //The digital pins that control the switches on the amplifier board. Used to calibrate the strain signals.
int ampMUXPinNum = 2;
int I2CMUXPins[6] = {16,17,18,19,20,21}; //The digital pins that control which leg the I2C connection is connected to. Used for calibrating the strain signals
int I2CMUXPinNum = 6;
byte wiperVal = 50; //The wiper value for the digipot from which to start the calibration
float allSGValues[6][6] = {}; //Empty array for the strain data
float value;
void setup() {
Serial.begin(115200);
while(!Serial); //Initialize Serial and don't continue until it's ready
Wire.begin(); //Initialize Wire to be able to communicate with the digipots over I2C
// Set all digital pins on the microcontroller as OUTPUTS and set them LOW
for(int i=0;i<MUXPinNum;i++)
{
pinMode(MUXPins[i], OUTPUT);
digitalWrite(MUXPins[i], LOW);
}
for(int i=0;i<ampMUXPinNum;i++)
{
pinMode(ampMUXPins[i], OUTPUT);
digitalWrite(ampMUXPins[i], LOW);
}
for(int i=0;i<I2CMUXPinNum;i++)
{
pinMode(I2CMUXPins[i], OUTPUT);
digitalWrite(I2CMUXPins[i], LOW);
}
//Set the chosen analog ports as inputs
for(int i=0;i<numAnalogPorts;i++)
{
pinMode(analogPorts[i], INPUT);
}
// Wait until MATLAB code startup is complete and servos are on, then calibrate each of the strain gauges
Serial.println("Waiting on MATLAB...");
int waiting = 1;
while(waiting == 1)
{
if(Serial.available()>0) //Check if anything has been sent by MATLAB over Serial
{
value = Serial.read(); //If there has been, read it
}
if(value>0) //If that value is something other than zero, then we have the go-ahead to start calibration from the MATLAB script
{
waiting = 0; //Stop waiting
Serial.println("Starting calibration.");
}
}
// CALIBRATION CODE
for(int L=0;L<numAnalogPorts;L++) //FOR each each leg...
{
digitalWrite(I2CMUXPins[L], HIGH); // Set the appropriate pin for the I2C analog switches HIGH to direct the I2C channels to your desired leg
for(int i=0;i<ampMUXPinNum;i++) //FOR each possible state of the switches on the amplifier board (corresponding to either "side" of the board)...
{
wiperVal = 50; //Begin the wiper at its midpoint
digitalWrite(ampMUXPins[i], HIGH); //Set the corresponding pin of the switch HIGH to flip the switch and direct the I2C channels to the desired "side" of the board
for(int j=0;j<numDigiPotIDs;j++) //FOR each of the unique digipot IDs...
{
int currMUXID = j+(3*i); //Calculate which strain signal ID (from 0 to 6 as notated on the amplifier boards) we're on currently based on the "side" of the board and the digipot ID
digitalWrite(MUXPins[currMUXID], HIGH); //Set the MUX pin HIGH to connect the strain signal to that leg's dedicated analog port
allSGValues[L][currMUXID] = analogRead(analogPorts[L]); //Collect the strain signal for that leg in the strain gage data array
while(allSGValues[L][currMUXID] < targetStrainBaseline - strainBaselineErrorMargin || allSGValues[L][currMUXID] > targetStrainBaseline + strainBaselineErrorMargin) //WHILE the strain value isn't within our desired calibration range...
{
Wire.beginTransmission(digiPotIDs[j]); //Prepare to send a new wiper value to the digipot
Wire.write(byte(0x00));
if(allSGValues[L][currMUXID] < targetStrainBaseline - strainBaselineErrorMargin) //IF the strain value is less than the desired level, increment the wiper value
{
wiperVal++;
}
else if(allSGValues[L][currMUXID] > targetStrainBaseline + strainBaselineErrorMargin) //ELSE decrement the wiper value to lower the strain
{
wiperVal--;
}
Wire.write(wiperVal); //Finish sending the new wiper value to the digipot
Wire.endTransmission();
allSGValues[L][currMUXID] = analogRead(analogPorts[L]); //Retake the strain data to see the effect of the wiper change. Repeat until you hit the desired calibration range
}
// Set all of the pins you set HIGH back to LOW to start the loops again
digitalWrite(MUXPins[currMUXID], LOW);
}
digitalWrite(ampMUXPins[i], LOW);
}
digitalWrite(I2CMUXPins[L], LOW);
Serial.print("Leg ");
Serial.print(L);
Serial.println(" complete.");
}
Serial.println("Done."); //Let MATLAB know you're done calibrating
}
void loop() {
//Only read values and broadcast them when MATLAB asks for it by sending a value ~=0 over Serial
if(Serial.available()>0)
{
value = Serial.read();
}
if(value>0)
{
//If the MATLAB script wants Serial data, cycle through which digital pin is HIGH and collect the strain data from each leg for each state of the switches. By doing so, you can collect strain from each location on the leg for all legs simultaneously, then switch
//which location you're collecting from.
for(int i=0;i<MUXPinNum;i++)
{
digitalWrite(MUXPins[i], HIGH);
for(int L=0;L<numAnalogPorts;L++)
{
allSGValues[L][i] = analogRead(analogPorts[L]);
}
digitalWrite(MUXPins[i], LOW);
}
//Print all of the collected data as one big line in Serial with a space between each point of data. The MATLAB script can then parse this data accordingly for each leg and location.
for(int L=0;L<numAnalogPorts;L++)
{
for(int i=0;i<MUXPinNum;i++)
{
Serial.print(allSGValues[L][i]);
Serial.print(" ");
}
}
//Print the carriage return to tell the MATLAB script that this is one complete timestep of data
Serial.write(13);
Serial.write(10);
//Delay for 2ms then start the read/print cycle again
delay(2);
}
}
%Clarus Goldsmith
%West Virginia University
% TO RECORD STRAIN, RUN 'collectDrosophibotStrain36MUX' IN ARDUINO BEFORE
% RUNNING THIS PROGRAM
clear all
close all
legNumbers = {[1 2 3], [4 5 6], [7 8 9], [10 11 12], [13 14 15 16 17], [18 19 20 21 22]};
motorColLabels = {'LH CTr', 'LH TrF', 'LH FTi',...
'RH CTr', 'LH TrF', 'LH FTi',...
'LM CTr', 'LM TrF', 'LM FTi',...
'RH CTr', 'LH TrF', 'LH FTi',...
'LF ThC1', 'LF ThC3', 'LF CTr', 'LF TrF', 'LF FTi',...
'RF ThC1', 'RF ThC3', 'RF CTr', 'RF TrF', 'RF FTi'};
U2D2Port = 'COM3'; % The port that the U2D2 is using
arduinoPort = "COM4";
numSteps = input('Desired number of steps?: ');
numStepsSuccess = 0;
while numStepsSuccess == 0
if rem(numSteps,1) == 0
if numSteps >= 1
numStepsSuccess = 1;
else
warning('Invalid entry. Must be an integer greater than or equal to 1.')
numSteps = input('Desired number of steps?: ');
end
else
warning('Invalid entry. Must be an integer.')
numSteps = input('Desired number of steps?: ');
end
end
backward = 0;
pertYN = input('Trial with perturbation (Y/N)?: ',"s");
pertStateSuccess = 0;
while pertStateSuccess == 0
if strcmp(pertYN, 'Y') || strcmp(pertYN, 'y') || strcmp(pertYN, 'N') || strcmp(pertYN, 'n')
if strcmp(pertYN, 'Y') || strcmp(pertYN, 'y')
pert = 1;
pertLeg = input('What leg should be perturbed (1-6)?: ');
pertSuccess = 0;
while pertSuccess == 0
if rem(pertLeg,1) == 0
if pertLeg <= 6 && pertLeg >= 1
pertSuccess = 1;
else
warning('Invalid entry. Must be an integer between 1-6.')
pertLeg = input('What leg should be perturbed (1-6)?: ');
end
else
warning('Invalid entry. Must be an integer between 1-6.')
pertLeg = input('What leg should be perturbed (1-6)?: ');
end
end
pertStep = input('During what step should the perturbation take place?: ');
pertSuccess = 0;
while pertSuccess == 0
if rem(pertStep,1) == 0
if pertStep <= numSteps && pertStep >= 1
pertSuccess = 1;
else
warning('Invalid entry. Perturbation must be within step number defined by numSteps.')
pertStep = input('During what step should the perturbation take place?: ');
end
else
warning('Invalid entry. Must be an integer.')
pertStep = input('During what step should the perturbation take place?: ');
end
end
else
pert = 0;
end
pertStateSuccess = 1;
else
warning('Invalid entry. Must be Y/N.')
pertYN = input('Trial with perturbation (Y/N)?: ',"s");
end
end
% substrate = 'wire mesh';
% substrate = 'anti-slip mat';
substrate = 'sand paper';
% simulationPath = "G:\Other computers\NeuroMINT Lab Computer\MATLAB\Design Drosophibot\Drosophibot 2\Flat\k_spring = 0\T_swing = 0.8\phi_I = 0.5\phi_C = 0.5\allVars.mat";
simulationPath = "G:\Other computers\NeuroMINT Lab Computer\MATLAB\Design Drosophibot\Drosophibot 2 Alt Hind Legs\Flat\k_spring = 0\T_swing = 0.8\phi_I = 0.5\phi_C = 0.5\allVars.mat";
% simulationPath = "C:\Users\Clarissa G\Documents\MATLAB\Design Drosophibot\Drosophibot 2 Alt Hind Legs\Flat\k_spring = 0\T_swing = 0.8\phi_I = 0.5\phi_C = 0.5\allVars.mat";
savePath = 'G:\Other Computers\NeuroMINT Lab Computer\MATLAB\Running Drosophibot\Trials\';
%% Extract the goal positions based on a trial of solver data
%Load the necessary variables from a save of the Design Drosophibot code
vars = {'numJoints','numLegs','numSegs','n','actuator','A','thetaPlot','Tactuator','stepPeriod','stanceDuty','stepHeight','floorLevel','springK','contralateralPhase','ipsalateralPhase','bodyTranslation','configName','terrainShape'};
load(simulationPath,vars{1:9})
solverParams = load(simulationPath,vars{7:end});
solverParams.solverPath = simulationPath;
%Put the thetas into a big matrix where each column corresponds to the
%appropriate servo ID + 1
goalPositions = zeros(n,numJoints);
goalTorques = zeros(n,numJoints);
i = 1;
for l=1:numLegs
for j=1:numSegs(l)
if norm(thetaPlot{l}(:,j)) ~= 0
goalPositions(:,i) = thetaPlot{l}(:,j);
goalTorques(:,i) = Tactuator{l}(:,j);
i = i+1;
end
end
end
stanceIDs = [];
%Find at which timestep all legs are in stance in the kinematics data
for t=1:length(A)/2
[Arows(t),~] = size(A{t});
if Arows(t) < 200
stanceIDs = [stanceIDs t];
end
end
%Use this time to identify how much the kinematics should be shifted by
shiftAmnt = mean(stanceIDs);
shiftAmnt = shiftAmnt-1;
%Bias the goalPositions based on the calculated torques and the
%stiffnesses
goalPositionsBiased = goalPositions + goalTorques/actuator.k;
%Convert the thetas from radians into servo bits
%From radians to deg:
goalPositions = goalPositions*180/pi;
goalPositionsBiased = goalPositionsBiased*180/pi;
%From deg to bits:
goalPositions = goalPositions/.088;
goalPositionsBiased = goalPositionsBiased/.088;
goalPositions = round(goalPositions);
goalPositionsBiased = round(goalPositionsBiased);
goalPositions = goalPositions + 2048;
goalPositionsBiased = goalPositionsBiased + 2048;
%Find how we have to downsample the data to make sure the bandwidth will not be too fast for
%the U2D2 to handle
minTimeStep = 0.03;
idealTimeStep = stepPeriod/n;
x = ceil(minTimeStep/idealTimeStep);
actTimeStep = idealTimeStep*x;
%Actually shift the data so all legs start in the middle of stance
goalPositions = circshift(goalPositions,shiftAmnt,1);
goalPositionsBiased = circshift(goalPositionsBiased,shiftAmnt,1);
numIters = ceil((stepPeriod*numSteps)/actTimeStep)+1; %Calculate the number of iterations based on the read/write timestep
%Initialize indexing variables
index = 2;
indexCont = 2;
servoIndex = 2;
strainIndex = 1;
strainStepInds = [];
simInds = 1;
s = 1;
pertInds = [];
%Chain the goal positions together for the number of steps desired
dxl_goal_position(1,:) = goalPositionsBiased(1,:);
% dxl_goal_position(1,:) = goalPositions(1,:);
applyPert = 0;
pertCount = 1;
while indexCont <= numIters
dxl_goal_position(indexCont,:) = goalPositionsBiased(index,:);
% dxl_goal_position(indexCont,:) = goalPositions(index,:);
if applyPert && pertCount <= 5
dxl_goal_position(indexCont,legNumbers{pertLeg}(1)) = dxl_goal_position(indexCont,legNumbers{pertLeg}(1))-1000;
dxl_goal_position(indexCont,legNumbers{pertLeg}(end)) = dxl_goal_position(indexCont,legNumbers{pertLeg}(end))+1000;
pertInds = [pertInds indexCont];
pertCount = pertCount+1;
end
simInds = [simInds index];
index = index+x;
if index > n
index = index - n;
s = s+1;
if pert
if s == pertStep
applyPert = 1;
end
end
end
indexCont = indexCont+1;
end
if backward %If we want to walk backward, flip the goalPositions
dxl_goal_position = flip(dxl_goal_position);
end
strainYN = input('Record Strain (Y/N)?: ',"s");
strainInSuccess = 0;
while strainInSuccess == 0
if strcmp(strainYN, 'Y') || strcmp(strainYN, 'y') || strcmp(strainYN, 'N') || strcmp(strainYN, 'n')
if strcmp(strainYN, 'Y') || strcmp(strainYN, 'y')
strainBool = 1;
else
strainBool = 0;
end
strainInSuccess = 1;
else
warning('Invalid input. Must be Y/N.')
strainYN = input('Record Strain (Y/N)?: ',"s");
end
end
%% Run the robot trial
stepNum = 1;
pertGo = 0;
if strainBool
%Labels for each column of the strain data
strainColLabels = {'LH Troch. Axial','LH Troch. Transverse', 'LH Troch. 45^{\circ}', 'LH Femur +45^{\circ}', 'LH Femur -45^{\circ}','LH Femur Axial',...
'RH Troch. Axial','RH Troch. Transverse', 'RH Troch. 45^{\circ}', 'RH Femur +45^{\circ}', 'RH Femur -45^{\circ}','RH Femur Axial',...
'LM Troch. Axial','LM Troch. Transverse', 'LM Troch. 45^{\circ}', 'LM Femur +45^{\circ}', 'LM Femur -45^{\circ}','LM Femur Axial',...
'RM Troch. Axial','RM Troch. Transverse', 'RM Troch. 45^{\circ}', 'RM Femur +45^{\circ}', 'RM Femur -45^{\circ}','RM Femur Axial',...
'LF Troch. Axial','LF Troch. Transverse', 'LF Troch. 45^{\circ}', 'LF Femur +45^{\circ}', 'LF Femur -45^{\circ}','LF Femur Axial',...
'RF Troch. Axial','RF Troch. Transverse', 'RF Troch. 45^{\circ}', 'RF Femur +45^{\circ}', 'RF Femur -45^{\circ}','RF Femur Axial'};
%Define the arduino serial connection information
arduinoObj = serialport(arduinoPort,115200);
configureTerminator(arduinoObj,"CR/LF");
flush(arduinoObj);
end
pause('on')
lib_name = '';
%Determine which specific library we need based on operating system
if strcmp(computer, 'PCWIN')
lib_name = 'dxl_x86_c';
elseif strcmp(computer, 'PCWIN64')
lib_name = 'dxl_x64_c';
elseif strcmp(computer, 'GLNX86')
lib_name = 'libdxl_x86_c';
elseif strcmp(computer, 'GLNXA64')
lib_name = 'libdxl_x64_c';
elseif strcmp(computer, 'MACI64')
lib_name = 'libdxl_mac_c';
end
% Load libraries for Dynamixels
if ~libisloaded(lib_name)
[notfound, warnings] = loadlibrary(lib_name, 'dynamixel_sdk.h', 'addheader', 'port_handler.h', 'addheader', 'packet_handler.h', 'addheader', 'group_sync_write.h', 'addheader', 'group_sync_read.h');
end
% Control table addresses
ADDR_TORQUE_ENABLE = 64; % Control table address is different in Dynamixel model
ADDRGOAL_POSITION = 116;
ADDR_PRESENT_POSITION = 132;
ADDR_PRESENT_LOAD = 126;
ADDR_PROFILE_VELOCITY = 112;
% Data Byte Lengths
LEN_GOAL_POSITION = 4;
LEN_PRESENT_POSITION = 4;
LEN_PRESENT_LOAD = 2;
LEN_PROFILE_VELOCITY = 4;
% Protocol version
PROTOCOL_VERSION = 2.0; % See which protocol version is used in the Dynamixel
ids = linspace(0,numJoints-1,numJoints);
DXL_ID = ids;
BAUDRATE = 1000000;
TORQUE_ENABLE = 1; % Value for enabling the torque
TORQUE_DISABLE = 0; % Value for disabling the torque
DXL_MOVING_STATUS_THRESHOLD = 5; % Dynamixel moving status threshold
DXL_PROFILE_VELOCITY = 62*2;
ESC_CHARACTER = 'e'; % Key for escaping loop
COMM_SUCCESS = 0; % Communication Success result value
COMM_TX_FAIL = -1001; % Communication Tx Failed
% Initialize PortHandler Structs
% Set the port path
% Get methods and members of PortHandlerLinux or PortHandlerWindows
port_num = portHandler(U2D2Port);
% Initialize PacketHandler Structs
packetHandler();
% Initialize Groupsyncwrite Structs
groupwrite_num = groupSyncWrite(port_num, PROTOCOL_VERSION, ADDRGOAL_POSITION, LEN_GOAL_POSITION);
% Initialize Groupsyncread Structs for Present Position
groupread_num = groupSyncRead(port_num, PROTOCOL_VERSION, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION);
groupread_num2 = groupSyncRead(port_num, PROTOCOL_VERSION, ADDR_PRESENT_LOAD, LEN_PRESENT_LOAD);
dxl_comm_result = COMM_TX_FAIL; % Communication result
dxl_addparam_result = false; % AddParam result
dxl_getdata_result = false; % GetParam result
dxl_error = 0; % Dynamixel error
%Create empty vectors for the loads and positions
dxl_present_position = zeros(numIters,numJoints);
dxl_present_load = zeros(numIters,numJoints);
timeVec = zeros(1,numIters);
timeVecInds = 1;
timeVecStrain = zeros(1,numIters*5);
strainDataRaw = strings(numIters*5,1);
earlyTerm = 0;
% Open the U2D2 port
if (openPort(port_num))
fprintf('Succeeded to open the port!\n');
else
unloadlibrary(lib_name);
fprintf('Failed to open the port!\n');
input('Press any key to terminate...\n');
return;
end
% Set U2D2 port baudrate
if (setBaudRate(port_num, BAUDRATE))
fprintf('Succeeded to change the baudrate!\n');
else
unloadlibrary(lib_name);
fprintf('Failed to change the baudrate!\n');
input('Press any key to terminate...\n');
return;
end
for i=1:numJoints
% Enable Dynamixel Torques
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID(i), ADDR_TORQUE_ENABLE, TORQUE_ENABLE);
dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION);
if dxl_comm_result ~= COMM_SUCCESS
fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
elseif dxl_error ~= 0
fprintf('%s\n', getRxPacketError(PROTOCOL_VERSION, dxl_error));
else
fprintf('Dynamixel #%d has been successfully connected \n', DXL_ID(i));
end
%Set Dynamixel velocities
write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID(i), ADDR_PROFILE_VELOCITY, DXL_PROFILE_VELOCITY);
dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION);
if dxl_comm_result ~= COMM_SUCCESS
fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
elseif dxl_error ~= 0
fprintf('%s\n', getRxPacketError(PROTOCOL_VERSION, dxl_error));
else
fprintf('Set velocity in Dynamixel #%d successfully \n', DXL_ID(i));
end
dxl_addparam_result = groupSyncReadAddParam(groupread_num, DXL_ID(i));
if dxl_addparam_result ~= true
fprintf('[ID:%03d] groupSyncRead addparam failed', DXL_ID(i));
return;
end
dxl_addparam_result = groupSyncReadAddParam(groupread_num2, DXL_ID(i));
if dxl_addparam_result ~= true
fprintf('[ID:%03d] groupSyncRead addparam failed', DXL_ID(i));
return;
end
end
%Initialize the joints at the first position in goalPositions
if input('Press any key to continue! (or input e to quit!)\n', 's') == ESC_CHARACTER
earlyTerm = 1;
else
for i=1:numJoints
% Add Dynamixel goal position value to the Syncwrite storage
dxl_addparam_result = groupSyncWriteAddParam(groupwrite_num, DXL_ID(i), typecast(int32(dxl_goal_position(1,i)), 'uint32'), LEN_GOAL_POSITION);
if dxl_addparam_result ~= true
fprintf('[ID:%03d] groupSyncWrite addparam failed', DXL_ID(i));
return;
end
end
groupSyncWriteTxPacket(groupwrite_num);
dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
if dxl_comm_result ~= COMM_SUCCESS
fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
end
% Clear syncwrite parameter storage
groupSyncWriteClearParam(groupwrite_num);
% Syncread present position
groupSyncReadTxRxPacket(groupread_num);
dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
if dxl_comm_result ~= COMM_SUCCESS
fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
end
%Syncread present load
groupSyncReadTxRxPacket(groupread_num2);
dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
if dxl_comm_result ~= COMM_SUCCESS
fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
end
% Get each Dynamixel present position and load value
for i=1:numJoints
dxl_present_position(1,i) = groupSyncReadGetData(groupread_num, DXL_ID(i), ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION);
dxl_present_load(1,i) = groupSyncReadGetData(groupread_num2, DXL_ID(i), ADDR_PRESENT_LOAD, LEN_PRESENT_LOAD);
end
end
%Warm up tic/toc
for ii=1:100
t_wus = tic;
t_wuf = toc(t_wus);
end
if strainBool
if ~earlyTerm
if input('Press any key to calibrate the strain gauges. (or input e to quit!)\n', 's') == ESC_CHARACTER
earlyTerm = 1;
else
write(arduinoObj,1,"uint8"); %Tell the arduino to start the calibration
data = readline(arduinoObj) %Intentionally left without semicolon to report out the 'Done' provided by the Arduino script
loopDone = 0;
while loopDone == 0
data = readline(arduinoObj)
if strcmp(data,"Done.")
loopDone = 1;
elseif isempty(data)
loopDone = 1;
earlyTerm = 1;
warning('Calibration not completed. Check strain gauge cables on leg for disconnections/breakages.')
input('Press any key to terminate...\n');
end
end
end
write(arduinoObj,0,"uint8"); %Tell the arduino to stop streaming data over Serial
fprintf('Strain gauge calibration complete. \n')
end
if ~earlyTerm
if input('Press any key to collect zero data for the strain gauges. (or input e to quit!)\n', 's') == ESC_CHARACTER
earlyTerm = 1;
else
write(arduinoObj,1,"uint8"); %Tell the arduino to start streaming data over Serial
for sd = 1:20
t_strs = tic;
%Collect 20 points of strain data to average to find where
%the unloaded value is
strainDataRawZero(sd,:) = readline(arduinoObj);
t_strf(sd) = toc(t_strs);
end
t_str = mean(t_strf);
end
write(arduinoObj,0,"uint8"); %Tell the arduino to stop streaming data over Serial
fprintf('Strain gauge zero data collected. \n')
end
end
if ~earlyTerm
if input('Press any key to run the robot! (or input e to quit!)\n', 's') == ESC_CHARACTER
earlyTerm = 1;
end
end
loopTime = [];
%% Begin loop for running the whole stepping
if ~earlyTerm
if strainBool
write(arduinoObj,1,"uint8"); %Tell arduino to start streaming data over Serial
end
if strainBool %If recording strain, record it to get a baseline number before moving.
strainDataRaw(strainIndex,:) = readline(arduinoObj);
timeVecStrain(strainIndex) = 0;
strainIndex = strainIndex + 1;
strainStepInds(strainIndex) = simInds(1);
end
fr = 2;
t_s = tic;
while fr <= numIters
t_ls = tic;
for i=1:numJoints
% Add Dynamixel goal position value to the Syncwrite storage
dxl_addparam_result = groupSyncWriteAddParam(groupwrite_num, DXL_ID(i), typecast(int32(dxl_goal_position(fr,i)), 'uint32'), LEN_GOAL_POSITION);
if dxl_addparam_result ~= true
fprintf('[ID:%03d] groupSyncWrite addparam failed', DXL_ID(i));
return;
end
end
% Syncwrite goal position
groupSyncWriteTxPacket(groupwrite_num);
dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
if dxl_comm_result ~= COMM_SUCCESS
fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
end
% Clear syncwrite parameter storage
groupSyncWriteClearParam(groupwrite_num);
% Syncread present position
groupSyncReadTxRxPacket(groupread_num);
dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
if dxl_comm_result ~= COMM_SUCCESS
fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
end
%Syncread present load
groupSyncReadTxRxPacket(groupread_num2);
dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
if dxl_comm_result ~= COMM_SUCCESS
fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
end
% Get each Dynamixel present position and load value
for i=1:numJoints
dxl_present_position(servoIndex,i) = groupSyncReadGetData(groupread_num, DXL_ID(i), ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION);
dxl_present_load(servoIndex,i) = groupSyncReadGetData(groupread_num2, DXL_ID(i), ADDR_PRESENT_LOAD, LEN_PRESENT_LOAD);
end
timeVec(servoIndex) = toc(t_s);
timeVecInds(servoIndex) = simInds(fr);
servoIndex = servoIndex + 1;
t_lf = toc(t_ls);
while t_lf < actTimeStep
if strainBool %If recording strain, record it.
if t_lf < actTimeStep - t_str*3
strainDataRaw(strainIndex,:) = readline(arduinoObj);
timeVecStrain(strainIndex) = toc(t_s);
strainIndex = strainIndex + 1;
strainStepInds(strainIndex) = simInds(fr);
end
end
t_lf = toc(t_ls);
end
loopTime = [loopTime, t_lf];
fr = fr+1;
end
end
%% Once the trial is done, turn off the robot, post-process the data and save it
if ~earlyTerm
input('Press any key to disable torques. \n')
end
% Disable each Dynamixel Torque
for i=1:numJoints
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID(i), ADDR_TORQUE_ENABLE, TORQUE_DISABLE);
dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION);
if dxl_comm_result ~= COMM_SUCCESS
fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
elseif dxl_error ~= 0
fprintf('%s\n', getRxPacketError(PROTOCOL_VERSION, dxl_error));
end
end
if strainBool
%Stop the arduino board transmitting over serial and close the port
write(arduinoObj,0,"uint8");
clear arduinoObj
end
% Close U2D2 port
closePort(port_num);
% Unload Library
unloadlibrary(lib_name);
if ~earlyTerm
saveBool = input('Save the current trial (1 for yes, 0 for no): ');
if saveBool
dxl_present_load_pp = dxl_present_load;
%Convert the torque values into meaningful numbers
for i=1:length(dxl_present_load)
for j=1:22
if dxl_present_load(i,j) > 60000
dxl_present_load_pp(i,j) = dxl_present_load(i,j) - 2^16;
end
end
end
for k=1:22
if any(k == [1 4 7 10 15 20])
dxl_present_load_pp(:,k) = dxl_present_load_pp(:,k)/1941;
dxl_present_load_pp(:,k) = dxl_present_load_pp(:,k)*6;
else
dxl_present_load_pp(:,k) = dxl_present_load_pp(:,k)*.001;
dxl_present_load_pp(:,k) = dxl_present_load_pp(:,k)*2.5;
end
end
if strainBool
for k=1:length(strainDataRawZero)
strainDataSepZero = split(strainDataRawZero(k,:),' ')';
for kk=1:36
strainDataRawNumsZero(k,kk) = str2double(strainDataSepZero(1,kk));
end
end
for k=1:length(strainDataRaw)
strainDataSep = split(strainDataRaw(k,:),' ')';
for kk=1:length(strainDataSep)
strainDataRawNums(k,kk) = str2double(strainDataSep(1,kk));
end
end
end
strainDataRawNums = strainDataRawNums(:,1:36);
strainDataRawNumsZero = strainDataRawNumsZero(:,1:36);
trialInfo = cell(9,1);
if ~backward
trialInfo{1} = 'Forward walking';
else
trialInfo{1} = 'Backward walking';
end
trialInfo{2} = ['Substrate: ' substrate];
trialInfo{3} = [num2str(stepPeriod) ' s step'];
if strainBool
trialInfo{4} = 'Strain Recorded?: Yes';
else
trialInfo{4} = 'Strain Recorded?: No';
end
if pert
trialInfo{5} = 'Perturbation?: Yes';
trialInfo{6} = ['Pert. Leg: ' num2str(pertLeg)];
trialInfo{7} = ['Pert. Step: ' num2str(pertStep)];
else
trialInfo{5} = 'Perturbation?: No';
end
trialInfo{8} = ['Actuator Profile Velocity: ' num2str(DXL_PROFILE_VELOCITY)];
note = input('Additional notes about this trial: ','s');
trialInfo{end} = ['Additional Notes: ' note];
tab = table(trialInfo);
date = datetime;
date.Format = 'yyyy-MM-dd HH_mm_ss';
currDate = convertStringsToChars(string(date));
mkdir(savePath,currDate)
if pert
if strainBool
save([savePath currDate '\robotData.mat'],'motorColLabels','dxl_goal_position','dxl_present_position','actTimeStep','dxl_present_load_pp','simInds','numSteps','timeVec','timeVecInds','strainDataRaw','strainDataRawNums','strainDataRawZero','strainDataRawNumsZero','timeVecStrain','strainStepInds','pertInds','pertLeg','pertStep','solverParams','strainColLabels')
else
save([savePath currDate '\robotData.mat'],'motorColLabels','dxl_goal_position','dxl_present_position','actTimeStep','dxl_present_load_pp','simInds','numSteps','timeVec','timeVecInds','pertInds','pertLeg','pertStep','solverParams')
end
else
if strainBool
save([savePath currDate '\robotData.mat'],'motorColLabels','dxl_goal_position','dxl_present_position','actTimeStep','dxl_present_load_pp','simInds','numSteps','timeVec','timeVecInds','strainDataRaw','strainDataRawNums','strainDataRawZero','strainDataRawNumsZero','timeVecStrain','strainStepInds','solverParams','strainColLabels')
else
save([savePath currDate '\robotData.mat'],'motorColLabels','dxl_goal_position','dxl_present_position','actTimeStep','dxl_present_load_pp','simInds','numSteps','timeVec','timeVecInds','solverParams')
end
end
writetable(tab,[savePath currDate '\README.txt'])
%Write trial description to the master text file
writelines(['---- ' currDate ' ----'],[savePath '\TRIAL INDEX.txt'],WriteMode = 'append')
trialDesc = [trialInfo{3} '. ' trialInfo{1} '. ' note];
writelines(trialDesc,[savePath '\TRIAL INDEX.txt'],WriteMode = 'append')
writelines(' ',[savePath '\TRIAL INDEX.txt'],WriteMode = 'append')
end
end
