To collect data, we need to find a way to move data from the Arduino to your computer. It turns out the Arduino's memory is too small to hold much data, so collecting the data and sending it after an experiment is over is not an option. The data has to be sent in real time, as it is generated. The only usable link to your computer is the USB connection, which is acting as a serial device. Bluetooth is too slow. To send the data as it is collected we need to use a high baud rate (1000000). We can run a terminal emulator that can capture (log) whatever is sent from the Arduino. I use Putty, as it is a popular terminal emulator that runs on Linux, Windows, and Macs. There are many other terminal emulators that run on Windows, so if you are already familiar with one that can log the Arduino output, use that.
You need to set your Arduino program to use the same baud rate with "Serial.begin( 1000000 );" in your setup program.
You can type commands to the Arduino by typing into the Putty window.
In a perfect world, whenever you started Putty, the Arduino would reboot and you would be capturing its output. You would be able to download new programs using the Arduino IDE. This did not work on my Linux boxes. I had to have no Putty running while downloading a program using the Arduino IDE, and then start Putty to capture output. I had to kill the Putty in order to download a new program, and then restart Putty. Sometimes I had to press the reset button on the Nano Every (white button near the USB connector) or power cycle the Arduino (remove USB cable from laptop and turn battery off, and then re-plug in the USB cable and turn the battery back on) to get the Arduino to reboot and show a prompt when putty is started up again.
As an example, we will show how to collect data from motors driving the wheels with the robot upside down (so the robot doesn't actually go anywhere, the wheels just spin in the air).
Make sure your battery is fully charged.
Connect the USB cable between the Nano Every and your computer.
Here is an example program.
It collects data where on each trial the motor is driven with a single sinusoid
of a particular frequency.
Here is the initial printout:
This is the end of the printout after I press 'g'
This is what was saved in putty.log
I collected some data where on each trial the motor was driven with a
single sinusoid of a particular frequency. The programs and data mentioned below
are all in model-sinusoids.zip.
Here is the program I wrote to parse my output files: parse-data.c.
It assumes I rename putty.log to something like run33.log.
On linux I compile this program by typing "make".
I run the program with "./parse-data run33", assuming the putty.log file has been renamed run33.log.
The program creates a
directory with that name (in this case run33) and then takes each section of the putty log corresponding
to a trial running a sinusoid with a constant frequency and creates a
Matlab-readable data file with a name like f08x4 (for frequency 8.4).
The columns are: timestamp (in microseconds), left encoder, right encoder,
and command (in these experiments I sent the same command to both motors).
The program also makes a file named files.txt,
and stores the name of the data file, the frequency, the voltage at the
time the data was collected, the raw voltage, and the number of points
in the data file. The file "files.txt" is stored in the data directory.
I like to do my data processing and modeling in its own directory.
In the zip file model-sinusoids.zip
there is a directory called "modeling-directory". As you try different
approaches, or process different data, you may want to create separate
data processing and/or modeling directories with whatever name you choose.
It is useful to create a Matlab command file that
loads all the data files created above into Matlab,
so you don't have to do it manually.
In the modeling-directory
(to get there type "cd modeling-directory" on linux)
there is an example command generator: create-commands.c,
and the corresponding command output, created with the command "./create-commands run33".
This program creates a file of matlab commands,
in this case run33.m.
You can modify it to match your own purposes, or edit the script it produces.
Now you can start matlab.
Make sure Matlab's current directory is the modeling directory, and then type "run33".
This makes Matlab look for the file run33.m and execute the commands in it.
The program process.m converts
the encoder readings into radians. A full revolution (2*pi radians) is
1560 counts using the Arduino encoder library. The gear ratio is 30, so that
means there are 13 counts per revolution, and the encoder software counts
every transition, so there are effectively 4x13=52 counts per revolution,
and 1560=30x52.
The program plots the difference of the microsecond clock, which should
always be 2000 (2 milliseconds). You can see there is a little variation:
The program plots the torque (blue) and angles (left is orange and right is
red-brown)
signal on the same scale.
When we start out at 1Hz, the torque and position are about the same
size on the graph (I scaled the torque to make this true).
The other two plots are for debugging. They show the FFT power plot.
The frequency, magnitude ratio, and phase lag data is stashed in an
array fff (I am not good on making up useful names).
The Bode plot figures for the motor command to wheel angle are
generated by the following commands (which are in plot_bode.m):
To save the frequency response data, so I can compare it to that of possible
models, I type:
Here is the ASCII (text) file fff33.txt and
the more compact binary file fff33.mat.
These files can be loaded into a new Matlab session using
These plots were generated by:
However, here is some contradictory evidence. Run 33 did the trial from
low to high frequencies, starting with a fully charged battery.
Run34 did the trials in the
reverse order, from high to low frequencies, also starting with a fully charged
battery.
The phase plots do show a dependence on voltage here, as do the
magnitude plots. Note that the center frequencies were done at about
the same voltages. A better experiment would be to do several sweeps
low to high frequencies as the voltage went down, to get a voltage
difference at the center frequencies.
These plots were generated using:
Unless we can model the dependence on voltage, the spread of these
frequency responses gives us an idea of how much uncertainty about
the model we should include.
Here is the sinusoidal data used above.
The columns in each data file
are: timestamp (in microseconds), left encoder, right encoder,
and command (in these experiments I sent the same command to both motors).
There is also a file named files.txt,
which stores the name of the data file, the frequency, the voltage at the
time the data was collected, the raw voltage, and the number of points
in the data file.
Parsing the output
You control the format of the output, so you can make it easy to parse.
Processing the data in Matlab
What do those Matlab commands do?
run33 is a collection of data files where each data file consists of driving
the system at a particular frequency.
The function process.m calculates the magnitude ratio and phase lag between
input command (torque) and outputs (left and right angles).
The overall method is to take
the FFT of both input and outputs,
and calculate the magnitude ratio and phase
lag at the frequency the system was driven at. process.m uses
fft2cga.m to call Matlab's FFT routine.
At a medium frequency (3.2Hz), the angle excursions
are reduced and more lagged relative to
the torque.
At a high frequency (15Hz), the angle excursions are almost too small to see, and more lagged.
figure(1)
loglog( fff(:,1), fff(:,2), '.' )
hold
loglog( fff(:,1), fff(:,4), '.' )
legend( 'left', 'right' )
title( "Magnitude ratio angle/torque (run33)" )
xlabel( "Log frequency (Hz)" )
ylabel( "Log ratio" )
figure(2)
semilogx( fff(:,1), -fff(:,3), '.' )
hold
semilogx( fff(:,1), -fff(:,5), '.' )
legend( 'left', 'right' )
title( "Phase lag of angle wrt torque (run33)" )
xlabel( "Log frequency (Hz)" )
ylabel( "Phase lag" )
There is good agreement between the left and right sides.
fff33 = fff;
save('fff33.txt','fff33','-ascii')
or
save('fff33.mat','fff33','-mat')
load fff33.txt
or
load fff33.mat
The variable fff33 will have the frequency response data for run33.
What changes as the battery runs down?
Run32 ran the battery down from 6.9 to 4.13. DO NOT DO THIS YOURSELF.
RECHARGE THE BATTERY ONCE IT GOES BELOW 7 VOLTS!!!! Running the battery
this low damages the battery. Fully recharge your battery early and
often.
The battery voltage is the 3rd column below (first is file name, second
is frequency). Once the battery voltage goes below 6.2volts, it collapses
to roughly 4V. From the run32 files.txt
../run32/f02x0 2.0 6.32 767 3000
../run32/f02x1 2.1 6.2 753 2951
../run32/f02x2 2.2 6.28 762 2901
../run32/f02x3 2.3 6.28 763 2851
../run32/f02x4 2.4 6.19 752 2801
../run32/f02x5 2.5 4.01 487 2751
../run32/f02x6 2.6 6.2 753 2701
../run32/f02x7 2.7 4.03 489 2651
../run32/f02x8 2.8 6.3 765 2601
../run32/f02x9 2.9 6.2 753 2550
../run32/f03x0 3.0 4.04 490 2501
../run32/f03x1 3.1 4.04 491 2452
../run32/f03x2 3.2 4.03 489 2400
../run32/f03x3 3.3 4.05 492 2351
../run32/f03x4 3.4 4.05 492 2301
../run32/f03x5 3.5 4.06 493 2251
../run32/f03x6 3.6 4.04 490 2201
../run32/f03x7 3.7 4.05 492 2151
../run32/f03x8 3.8 6.24 758 2101
../run32/f03x9 3.9 4.08 495 2051
../run32/f04x0 4.0 4.09 497 2001
Here are the Bode plots for left side for
run32 (fff32.txt or
fff32.mat)
and run33 plotted together. I stopped run32
early because of the low voltage, and fully recharged the battery before run33.
The low battery voltage lowered the magnitude plot by 15%.
It should have been lowered
by the ratio of the starting voltages: 6.9/8.3 (17%), which is close.
So it appears the voltage affects the magnitude plot linearly.
Above 2.5Hz we see the collapse to 4V reduces the magnitude more.
The voltage seems to have little effect on the phase plot until the collapse
to 4V, which we don't care about since we won't ever go this low again!
figure(1)
loglog( fff33(:,1), fff33(:,2), '.' )
hold
loglog( fff32(:,1), fff32(:,2), 'r.' )
title( "Magnitude ratio angle/torque" )
xlabel( "Log frequency (Hz)" )
ylabel( "Log ratio" )
legend('run33','run32')
figure(2)
semilogx( fff33(:,1), -fff33(:,3), '.' )
hold
semilogx( fff32(:,1), -fff32(:,3), '.' )
title( "Phase lag of angle wrt torque" )
xlabel( "Log frequency (Hz)" )
ylabel( "Phase lag" )
legend('run33','run32')
Here are the magnitude plots for both the left and right sides.
Here are the phase plots for both the left and right sides.
figure(1)
semilogx(fff33(:,1), fff33(:,6), '.' )
hold
semilogx(fff34(:,1), fff34(:,6), '.' )
title( "Voltage when the frequency sampled" )
xlabel( "Log frequency (Hz)" )
ylabel( "voltage" )
legend('run33','run34','Location','west')
figure(2)
loglog( fff33(:,1), fff33(:,2), '.' )
hold
loglog( fff34(:,1), fff34(:,2), '.' )
loglog( fff33(:,1), fff33(:,4), 'k.' )
loglog( fff34(:,1), fff34(:,4), 'm.' )
legend('run33 left','run34 left','run33 right','run34 right')
title( "Magnitude ratio angle/torque" )
xlabel( "Log frequency (Hz)" )
ylabel( "Log ratio" )
figure(3)
semilogx( fff33(:,1), -fff33(:,3), '.' )
hold
semilogx( fff34(:,1), -fff34(:,3), '.' )
semilogx( fff33(:,1), -fff33(:,5), 'k.' )
semilogx( fff34(:,1), -fff34(:,5), 'm.' )
legend('run33 left','run34 left','run33 right','run34 right')
title( "Phase lag of angle wrt torque" )
xlabel( "Log frequency (Hz)" )
ylabel( "Phase lag" )