User interface: Difference between revisions

From Rsewiki
No edit summary
 
(98 intermediate revisions by the same user not shown)
Line 1: Line 1:


==Screen dumps==
Back to [[regbot | Regbot]] main page


The user interface can configure and run the robot, as well as inspecting almost all values on the robot. Written in python using Qt GUI library.
==Overview==


The interface looks like on figures below.
Connection is either using USB or network if the robot has wifi.
 
On Windows USB is com3, com4 or any higher, see the device-list in the control panel.


On Linux USB is /dev/ttyACM0 or /dev/ttyACM1 if /dev/ttyACM0 is used for other devices.


[[File:Gui_robot.png]]


Figure 1. The general settings for the robot. The left panel is the general connection status and space for some messages from the robot. The central tab is mostly for configuration of the robot and some sensor and calculated values like pose and tilt. To the right is a fast graph of the last mission.
The user interface can configure and run the robot, as well as inspect almost all values on the robot. Written in python using Qt GUI library.


=== Data logger ===
The interface looks like on figures below.


[[File:Gui_rev0.png]]
In general:
* Yellow fields are read-only (updates from the robot when connected).
* White fields are editable
* Editable fields require (in general) that an "edit" button be pressed first and a "save" button after the edit.
* Many check-boxes get implemented as they are checked/unchecked, but not all.
* Data is updated from the robot at a relatively low update rate; if data is to be used for documentation, then use the onboard log function.


Figure 2. The data logging options. A number of sensor values and interface points in the robot software can be logged. The text window shows loaded data from a mission. The log format is designed to be directly compatible with the 'load' function in MATLAB.


The control details gives detaild at datapoints connected to or inside the general PID controler design used for all control settings. These control datapoints is illustrated in the figure below.
[[File:Gui_robot.png | 800px]]


== Manu ==
Figure 1. The general settings for the robot. The left panel is the general connection status and space for some messages from the robot. The central tab is primarily for the configuration of the robot and some sensors and calculated values like pose and tilt. To the right is a fast graph of the last mission.
NB! The data rate is low, so to get an overview, use the log function for more details.


[[File:regbot_gui_show_menu.png]]
=== Data logger ===


Figure 4. Other tab pages are available from the "show" menu.
[[File:Gui_rev0.png | 800px]]


==Mission==
Figure 2. The data logging options. A number of sensor values and interface points in the robot software can be logged. The text window shows loaded data from a mission. The log format is designed to be directly compatible with the 'load' function in MATLAB.


Mission specification consist of mission lines, each line consist of two (lower case) parts divided by ':'
====Log interval====


Burde formuleres efter EBNF
The sample interval is set in the mission and is visible here after a mission.
* mission = mission_line, { mission line };
* mission line = [drive parameter list], [':' , continue condition list];
*drive parameter list = ...


The short form:
==== Log data ====


drive parameter: continue condition (conditions are OR'ed)
Once the log is fetched, it can be saved to a file. The format is intended for MATLAB.


e.g.:
Use a script like this in MATLAB


  vel=-0.2, acc=3.0 : dist=1, time=12
  clear
close all
%  1    time 0.9460 sec, from Ella (8)
2 3  4  5  (mission 0), state 9, entered (thread 1, line 3), events 0x0 (bit-flags)
%  6  7 Motor velocity ref left, right: 0.0000 0.0000
%  8  9 Motor voltage [V] left, right: 0.03 -0.03
% 10 11 Motor current left, right [A]: -0.010 0.011
% 12 13 Encoder left, right: -420 414
% 14 17 Wheel velocity [m/s] left, right: -0.0031 0.0030 delay 0.1000 0.1000 (sec)
% 18 19 20 21 Pose x,y,h,tilt [m,m,rad,rad]: 0.169143 -0.00276473 -0.0162272 -0.250711
% 22    Battery voltage [V]
%%
data = '''load('log_position-liv36_b.txt')''';
%% plot motor voltage and velocity
figure(100)
hold off
plot(data(:,1), data(:,8), 'linewidth', 2)
hold on
plot(data(:,1), data(:,14), ':', 'linewidth', 2)
grid on
legend('left voltage [V]', 'left vel [m s^{-1}]', 'location','northwest');
xlabel('Time [sec]')


Drive backwards at a speed of 0.2m/s, accelerate with 3m/s2 for 1 meter (or max 12 seconds)
The top lines of the log file are copied into the MATLAB script to identify data columns.


=== Drive values ===
==== Log options ====


* VEL is velocity in m/s - positive is forward, 0=stop. uses last value if omitted.
Only tagged data items are logged (to save memory). See description below.


* ACC is acceleration limit in m/s2. Uses last value if omitted.
====ACC and gyro====


* TR is turnradius in metre - positive, 0 is turn on the spot. straight if omitted. NB! in balance the minimum turn radius is about 0.07m, as balance requires that at least one wheel can give a forward acceleration.
Acceleration logs the acceleration values in m/s^2 on all 3 axes (x (forward if in balance),y (left), z (up if in balance))


* LOG is log interval in milliseconds. Once started it continues until buffer is full.
Gyro is rotation velocity around the same 3-axis. Unit is degrees per second (after calibration offset).


* BAL is balancing, uses last value if omitted.
====Encoder====


* HEAD is reference heading in degrees (-180 .. 180), set at start of line.
Encoder values for wheel rotation (left, right) 48 values per rotation, increases on forward.


* LABEL is a label number that can be used by GOTO.
====Motor voltage====


* GOTO is a jump to the label number given. This can be limited to COUNT use of goto, the count is then reset after the first skip. The goto can also be skipped if any other conditions are true when this line is reached. A line with a goto will never wait.
Is the motor voltage - before conversion to PWM, compensated for changes in battery voltage.


====Line-sensor====
====Motor current====


If a line sensor is installed, then the following should work too:
Filtered current sensor values. zero current is calibrated just before the start of a mission - this calibration sometimes makes a mistake, resulting in a wrong offset of the logged current. Value is in Amps. The value is filtered to match the log interval if the interval is > 2ms.


* EDGER is following Right edge of line at -2..2 (in cm), positive is right
====Wheel velocity====


* EDGEL is following Left edge of line at -2..2 (in cm), positive is right
Based on the time between encoder tics. If more tics are within 1 ms, then the interval time is averaged. When there is a long time between tics (>50ms), then the time since the last tick is used to calculate velocity, i.e. velocity goes towards zero with time if no encoder pulses arrive.


* WHITE set to 1 if follow-line tape is white, else 0
==== Turn rate ====


====Distance sensor====
The turn rate is based on encoder tics (not the gyro), the unit is radian/sec


If IR distance sensors are installed, then these should work:
==== Robot pose ====


* IRSENSOR is IR-sensor to use (1 is distance to a wall, and 2 is distance in front).
The robot pose is position since the start of the mission (odometry coordinates), x,y,h,t where h is heading in radians (positive counterclockwise), and t is a tilt in radians (zero is in balance, positive is forward).


* IRDist is IR-distance to hold.
==== Line sensor ====


====Servos====
All values related to the line sensor, including AD value from each sensor.


Control of servos, there is support for up to 3 servos (version 3 only).
==== IR sensor ====
There is 2 extra pins, one analog pin DAC (called servo 4) (real D/A converter) and one digital pin (24) called servo 5.


* SERVO=N PSERVO=XXX where N is servo number (1..5) and XXX is servo position, for servo 1,2 and 3 value is in range +/- 1000, where 0 is 1.5 ms, +500 is 2ms, + 1000 is 2.5ms, -500 is 1ms and -1000 is 0.5ms pulse with with at pulse frequency of 333 Hz. For servo 4 value is analog value -1000 is minimum and +1000 is maximum. for servo 5 (digital pin) XXX=0 is Low (0V) and XXX=1 is High (3.3V).
Is distance converted to meters for each sensor (default is sensor 1 to the right and sensor 2 is forward).
==== Battery ====


====Threads====
Battery voltage in Volts.


A command sequence may be divided into threads. The default thread has number 1.
==== Extra ====


* THREAD=3 means that this and all following lines are handled by one thread (with number 3). All threads are activated at time 0.
Debug feature for log of extra values.


Example
==== Mission ====
thread=0
    log=20: log=0
    event=0
thread=2
    label=1
    vel=0.4: time=1.0
    vel=0.2: time=1.0
    vel=0.0: time=1.0
    event=3 : event=7
    vel=0.2, tr=0.15:turn=90
    goto=1
thread=9
    bal=0:event=3
    bal=1: time=2
    label=12
    event=7:time=0.1
    goto=12
thread=10
    label=20,servo=1,pservo=-300:time=1
    servo=1,pservo=200:time=1
    goto=20


Thread 0: This thread controls the logging function with the condition 'log=0' that is true when the log is full,
State, Thread, Line, Event. Where State is mission state (should always be 2 or 8 when stopping), thread number and line number in that thread, for the latest line activated (if more than one is activated in the same log interval, then the latest is logged. An event is a 32-bit integer where each bit corresponds to an event number, and events are accumulated over the logging period.
then the mission is stopped. 'event=0' is a special event that stops the mission.


Thread 2: This thread controls velocity in three steps 0.4m/s, 0.2 m/s and 0m/s, then sends an event (event=3) and waits for event 7.
==== Motor ref ====
When event 7 arrives then the robot turns 90 degrees and starts over.


Thread 9: This thread controls the balance. Starts with no balance, until event 3 occurs, then goes into balance mode and waits 2 seconds (to stabalize in balance). Then it enters a loop that emits event 7 every 0.1 second, to keep thread 2 going.
Is the desired motor velocity (input to velocity controller) - in m/s


Thread 10: this is a servo control thread, that moves servo 1 between 2 positions.
==== Stop logging when buffer is full ====


The mission is finished when all threads has ended, in this case when the log is full.
If this is unchecked, then the log buffer is overwritten until the mission is stopped.
This is useful when a later part of the mission is more interesting.


The thread number is pt. not used to determine the execution order of the threads (uses script order).
==== Control time ====


====Events====
Is the time taken to handle sensor data, calculate control values and advance in mission lines. All should be finished within the sample time. Value is in microseconds, so it should be below 1000 to ensure valid control calculations.


Events can be generated if an event=X command is entered in the parameter side of the colon. The event is activated when the line is activated.
==== Control details ====
More than one event can be activated in the same line, e.g.:


vel=0.2,event=7,event=12:time=1
Value order: r, m, m2, uf, r2, ep, up, ui, u1, u
event=0


Here event 7 and 12 are activated at the same time as the velocity is set to 0.2.
The control details give details at data points connected to or inside the general PID controller design used for all control settings. These control data points are illustrated in the figure below.


Event 0 has a special meaning and will terminate full mission NOW!
===Controller configuration===


Events are used in the example above.
All controllers can be configured from the "control" tab. See the [[Control]] page for more control details.


===Continue conditions===
[[File:Gui_control.png | 800px]]


':' is separation of parameters and continue condition.
Figure 4b. All control values are set from this page; click on the relevant controller and enter the relevant values in the dialogue window shown below.


* DIST is driven distance in this mission line - positive meters
[[File:Gui_control_dialog.png | 500px]]
* TURN is angle turned in this mission line - degrees, positive is Left (CCW if forward speed)
* TIME is max time in this mission line - positive seconds
* COUNT is used with GOTO and GOTO will be ignored, if this line is executed more than this count
* XB,XW,LV is conditions relevant for the line sensor (if implemented) - see below
* IR1,IR2 is conditions relevant for IR distance sensors - se below
* TILT is tilt angle (especially useful if not in balance.
* EVENT is testing for events
* LOG is true when log is full (value is ignored, but must be set to a number (e.g. 0))
* HEAD is absolute heading test, head=175 is true if actual heading is within 3 degrees of this value. head>XXX and head<YYY compares with current value, but is difficult to use when crossing the +/- 180 deg border.


The equal (=) sign can for some values (dist, turn, xb, xw and tilt) be replaced by '<' or '>', for most values (except xb, xw and head) an equal sign means equal or greater. NB! '>=', '==', '<=' and '!=' are not valid.
Figure 5. This dialogue configures the wheel velocity controller - shown as a PI-Lead-controller with feed-forward and an output limit at (+/-) 9V. All other options are disabled. The blue frame boxes indicate a traditional PID controller with lead in forward (and)/or in the feedback branch.


Example: Drive 0.2m then turn 30 deg to the right (turn radius=0, but will be more with acceleration being only 1m/s2) then drive another 1 second at a higher speed (or maximum 0.4m).
When "enable controller" is disabled, then the control is disabled.  


vel=0.2,acc=1 : dist=0.2
To use feed-forward only (i.e. set output directly from ref), enable controller, but set Kp=0, and enable feed-forward with some value for Kf (when Kf = 1, then ref is used directly as output).
tr=0 : turn = -30
vel=0.5 : dist=0.4,time=1


Example Zig-zag sideways: drive 0.2m backwards and then turn 45 deg left, 45 deg right and a bit forward, repeated 2 times more with a goto (to label=6).
NB! As velocity measurements are rather noisy, the filter in the feedback path (Lead/Lag feedback) could be implemented as a low-pass filter with tau_zero = 0 and tau_pole = 0.005 (recommended).


vel=-0.2, acc=2.0: dist=0.2
=== IMU===
label=6:
tr=0.15: turn=45.0
tr=0.15: turn=-45.0
vel=0.2:dist=0.18
vel=-0.2,goto=6: count=2


==== Line sensor ====
The IMU page displays IMU data. The balance controller uses the tilt measurement.


This section is valid if a line sensor is installed and calibrated. Use "Edge" tab for calibration (put edge sensor "ON" and on a dark background and press "Calibrate no reflection" and then on a bright background and press "Calibrate white reflection", then "save on robot").
[[File:Gui_IMU.png | 800px]]


* XB, XW is test for crossing black/white line, value is 0..20, 0 is true on no crossing, 1..20 is confidence in crossing (20 is highest)
Figure 5.1 IMU data. The graph shows calibrated values, and the gyro is calibrated by keeping the robot stable and pressing the "calibrate" button. (Remember to save the result in the robot flash.) The gyros drift slightly; repeat as needed.
* LV is test for valid line 0=true for no valid line, 1=true for valid line. NB! A wooden (or dirty) floor is likely to give a valid line at times.


Example: Drive until a crossing black line is found or 2.5m is driven, then continue until the crossing line is gone (reached other side of line) and stop
The IMU board orientation needs to be configured so that its tilt is zero when balanced.
For most robots, the tilt angle (about the Y-axis) should be slightly less than 180 degrees.


vel=0.2, log=5, acc=2: xb>16, dist=2.5
Hold the robot at the balance point, and see the tilt angle. This should be zero (or within +/- 1-2 degrees), if not adjust the y angle in the IMU board box (Edit, change, Apply and then "save to flash").
:xb < 4,dist=0.2
vel=0:time=0.1


Example find white line and follow edge: Drive until (white) line is found - as before - then turn to the right and follow line for 0.5 m.
=== Menu ===


vel=0.2, log=5, acc=2: xw>16, dist=2.5
:xw < 4,dist=0.2
vel=0:time=0.3
tr=0,vel=0.2:turn=-90
edgel=0, white=1: dist=0.5


NB! testing of crossing white or black is unsafe if in balance (requires good calibration in balance). And if the front is raised, then this feature do not work at all.


==== Distance sensor====
[[File:regbot_gui_show_menu.png | 600px]]


IR1, IR2 is test for distance measured by IR sensor.
Figure 6. Other tab pages are available from the "show" menu.


@todo Distance sensor is not tested much, and is therefore assumed not to work properly.
Some of the available data tabs are for debug and maintenance.


==== Tilt balance angle ====
===Mission===


TILT is test for tilt angle (0 is balance point)
Missions are entered through the mission tab:


Example:
[[File:Gui_mission.png | 700px]]
@todo


==== Events ====
Figure 7. Missions are entered in the left (white) area, and can be syntax tested with the button above.
The result of the check is shown in the yellow area (right).
The mission of the robot can be fetched to the right area.
Missions (the left area) can be saved and loaded from text files with the buttons below.


An event can be a test condition in a mission line, and can be generated by a mission line.
If you copy-paste from another application, so make sure not to include any formatting; 7-bit ASCII characters are allowed only.
Up to 32 events can be used, they are named "event=N", where N is a number in the range [0..31].
An event is valid for one sample period only (1ms).


Events can be created from the communication interface with the command like
Missions are not saved to the robot before you press "save to robot".
<event=5>


An example could be like this (a not very functional example)
The mission is lost for the robot after a power cycle unless you save the configuration to robot flash - using the "save to robot flash" in the left pane.


thread=2
==== Mission lines ====
  vel=0.2:time=1          # driving at speed 0.2 m/s for 1 second
  vel=0,event=7:event=5  # set speed to 0 and generate event 7, then wait for event 5
  vel=0.2:time=1          # speed back to 0.2 m/s in 1 second
  vel=0                  # stop this thread
thread=8
  label=1:time=5,event=7  # wait for event 7 (generated after 1 second by thread 2) or for maximum 5 seconds (should never happen)
  event=12:time=1        # generate event 12 (and wait for 1 second)
  event=13:time=1        # generate event 13 (and wait for 1 second before ending this thread


The mission will not end before the user has generated event 5 (by sending "<event=5>").
Mission specification consists of mission lines; each line consists of two (lower case) parts divided by ':'
Every time an event is generated a message is send to the client line, for this mission the following messages will be generated:


  # event 7
  drive parameter: continue condition
# event 12
# event 13
# event 5    (this in response to a user event)


Event 0 can't be tested, it will terminate the mission immediately.
continue conditions are OR'ed.


===Examples===
e.g.:


Example drive-turn-drive-turn at 20cm/s:  
vel=-0.2, acc=3.0 : dist=1, time=12


vel=0.2, acc=1.5 : dist = 0.5, time=5
Drive backwards at a speed of 0.2m/s, and accelerate with 3m/s2 to this speed until a distance of 1 meter is driven or 12 seconds have passed.
tr=0.15 : turn=90
: dist = 0.5
tr=0.15 : turn=90
vel=0 : time=1


Example wait 5 seconds:
See [[Mission]] for more details.


vel=0 : time=5
=== Motor test ===


Example balance and follow line: Go on balance and follow left edge of white line for 0.5m, then follow line for another 0.5m without balance.
There is a motor test option.


vel=0.2,edgel=0,white=1,bal=1: dist>0.5
[[file: gui-motortest-2025.png | 700px]]
bal=0,edgel=0,white=1:dist=0.5


NB! Most examples - and especially balance - requires valid controller parameters and calibrated sensors (tilt offset for balance).
The test runs each motor in two steps and writes a log file during execution.


===Save ===
The log file is analysed to determine the time required for the motor current to increase before the motor velocity changes. This is used to estimate the time constant for the motor inductance. The peak current is used to estimate the motor driver and motor coil resistance.
Transfer the mission to the robot RAM by pressing "Save" (top right), the mission will be lost by a reboot or power off, unless saved on robot!


Save the mission on the robot flash memory (EE-prom) by pressing "save on Robot" (left panel). NB! The space is limited (about 100 lines)!
The motor constant and dynamic friction are estimated from the steady state values of velocity and current.


The mission can also be saved in a text-file using the bottom right save button
There may be cases where the estimate fails; in these cases, the motor resistance typically shows 200 Ω. Try again. Also, try to run a mission before the motortest. There must be some data-handling issues that are not adequately addressed; this issue is not fixed yet (Dec 2025).

Latest revision as of 19:57, 30 December 2025

Back to Regbot main page

Overview

Connection is either using USB or network if the robot has wifi.

On Windows USB is com3, com4 or any higher, see the device-list in the control panel.

On Linux USB is /dev/ttyACM0 or /dev/ttyACM1 if /dev/ttyACM0 is used for other devices.


The user interface can configure and run the robot, as well as inspect almost all values on the robot. Written in python using Qt GUI library.

The interface looks like on figures below.

In general:

  • Yellow fields are read-only (updates from the robot when connected).
  • White fields are editable
  • Editable fields require (in general) that an "edit" button be pressed first and a "save" button after the edit.
  • Many check-boxes get implemented as they are checked/unchecked, but not all.
  • Data is updated from the robot at a relatively low update rate; if data is to be used for documentation, then use the onboard log function.


Figure 1. The general settings for the robot. The left panel is the general connection status and space for some messages from the robot. The central tab is primarily for the configuration of the robot and some sensors and calculated values like pose and tilt. To the right is a fast graph of the last mission. NB! The data rate is low, so to get an overview, use the log function for more details.

Data logger

Figure 2. The data logging options. A number of sensor values and interface points in the robot software can be logged. The text window shows loaded data from a mission. The log format is designed to be directly compatible with the 'load' function in MATLAB.

Log interval

The sample interval is set in the mission and is visible here after a mission.

Log data

Once the log is fetched, it can be saved to a file. The format is intended for MATLAB.

Use a script like this in MATLAB

clear
close all
%  1    time 0.9460 sec, from Ella (8)
%  2  3  4  5   (mission 0), state 9, entered (thread 1, line 3), events 0x0 (bit-flags)
%  6  7 Motor velocity ref left, right: 0.0000 0.0000
%  8  9 Motor voltage [V] left, right: 0.03 -0.03
% 10 11 Motor current left, right [A]: -0.010 0.011
% 12 13 Encoder left, right: -420 414
% 14 17 Wheel velocity [m/s] left, right: -0.0031 0.0030 delay 0.1000 0.1000 (sec)
% 18 19 20 21 Pose x,y,h,tilt [m,m,rad,rad]: 0.169143 -0.00276473 -0.0162272 -0.250711
% 22    Battery voltage [V]
%%
data = load('log_position-liv36_b.txt');
%% plot motor voltage and velocity
figure(100)
hold off
plot(data(:,1), data(:,8), 'linewidth', 2)
hold on
plot(data(:,1), data(:,14), ':', 'linewidth', 2)
grid on
legend('left voltage [V]', 'left vel [m s^{-1}]', 'location','northwest');
xlabel('Time [sec]')

The top lines of the log file are copied into the MATLAB script to identify data columns.

Log options

Only tagged data items are logged (to save memory). See description below.

ACC and gyro

Acceleration logs the acceleration values in m/s^2 on all 3 axes (x (forward if in balance),y (left), z (up if in balance))

Gyro is rotation velocity around the same 3-axis. Unit is degrees per second (after calibration offset).

Encoder

Encoder values for wheel rotation (left, right) 48 values per rotation, increases on forward.

Motor voltage

Is the motor voltage - before conversion to PWM, compensated for changes in battery voltage.

Motor current

Filtered current sensor values. zero current is calibrated just before the start of a mission - this calibration sometimes makes a mistake, resulting in a wrong offset of the logged current. Value is in Amps. The value is filtered to match the log interval if the interval is > 2ms.

Wheel velocity

Based on the time between encoder tics. If more tics are within 1 ms, then the interval time is averaged. When there is a long time between tics (>50ms), then the time since the last tick is used to calculate velocity, i.e. velocity goes towards zero with time if no encoder pulses arrive.

Turn rate

The turn rate is based on encoder tics (not the gyro), the unit is radian/sec

Robot pose

The robot pose is position since the start of the mission (odometry coordinates), x,y,h,t where h is heading in radians (positive counterclockwise), and t is a tilt in radians (zero is in balance, positive is forward).

Line sensor

All values related to the line sensor, including AD value from each sensor.

IR sensor

Is distance converted to meters for each sensor (default is sensor 1 to the right and sensor 2 is forward).

Battery

Battery voltage in Volts.

Extra

Debug feature for log of extra values.

Mission

State, Thread, Line, Event. Where State is mission state (should always be 2 or 8 when stopping), thread number and line number in that thread, for the latest line activated (if more than one is activated in the same log interval, then the latest is logged. An event is a 32-bit integer where each bit corresponds to an event number, and events are accumulated over the logging period.

Motor ref

Is the desired motor velocity (input to velocity controller) - in m/s

Stop logging when buffer is full

If this is unchecked, then the log buffer is overwritten until the mission is stopped. This is useful when a later part of the mission is more interesting.

Control time

Is the time taken to handle sensor data, calculate control values and advance in mission lines. All should be finished within the sample time. Value is in microseconds, so it should be below 1000 to ensure valid control calculations.

Control details

Value order: r, m, m2, uf, r2, ep, up, ui, u1, u

The control details give details at data points connected to or inside the general PID controller design used for all control settings. These control data points are illustrated in the figure below.

Controller configuration

All controllers can be configured from the "control" tab. See the Control page for more control details.

Figure 4b. All control values are set from this page; click on the relevant controller and enter the relevant values in the dialogue window shown below.

Figure 5. This dialogue configures the wheel velocity controller - shown as a PI-Lead-controller with feed-forward and an output limit at (+/-) 9V. All other options are disabled. The blue frame boxes indicate a traditional PID controller with lead in forward (and)/or in the feedback branch.

When "enable controller" is disabled, then the control is disabled.

To use feed-forward only (i.e. set output directly from ref), enable controller, but set Kp=0, and enable feed-forward with some value for Kf (when Kf = 1, then ref is used directly as output).

NB! As velocity measurements are rather noisy, the filter in the feedback path (Lead/Lag feedback) could be implemented as a low-pass filter with tau_zero = 0 and tau_pole = 0.005 (recommended).

IMU

The IMU page displays IMU data. The balance controller uses the tilt measurement.

Figure 5.1 IMU data. The graph shows calibrated values, and the gyro is calibrated by keeping the robot stable and pressing the "calibrate" button. (Remember to save the result in the robot flash.) The gyros drift slightly; repeat as needed.

The IMU board orientation needs to be configured so that its tilt is zero when balanced. For most robots, the tilt angle (about the Y-axis) should be slightly less than 180 degrees.

Hold the robot at the balance point, and see the tilt angle. This should be zero (or within +/- 1-2 degrees), if not adjust the y angle in the IMU board box (Edit, change, Apply and then "save to flash").

Menu

Figure 6. Other tab pages are available from the "show" menu.

Some of the available data tabs are for debug and maintenance.

Mission

Missions are entered through the mission tab:

Figure 7. Missions are entered in the left (white) area, and can be syntax tested with the button above. The result of the check is shown in the yellow area (right). The mission of the robot can be fetched to the right area. Missions (the left area) can be saved and loaded from text files with the buttons below.

If you copy-paste from another application, so make sure not to include any formatting; 7-bit ASCII characters are allowed only.

Missions are not saved to the robot before you press "save to robot".

The mission is lost for the robot after a power cycle unless you save the configuration to robot flash - using the "save to robot flash" in the left pane.

Mission lines

Mission specification consists of mission lines; each line consists of two (lower case) parts divided by ':'

drive parameter: continue condition 

continue conditions are OR'ed.

e.g.:

vel=-0.2, acc=3.0 : dist=1, time=12

Drive backwards at a speed of 0.2m/s, and accelerate with 3m/s2 to this speed until a distance of 1 meter is driven or 12 seconds have passed.

See Mission for more details.

Motor test

There is a motor test option.

The test runs each motor in two steps and writes a log file during execution.

The log file is analysed to determine the time required for the motor current to increase before the motor velocity changes. This is used to estimate the time constant for the motor inductance. The peak current is used to estimate the motor driver and motor coil resistance.

The motor constant and dynamic friction are estimated from the steady state values of velocity and current.

There may be cases where the estimate fails; in these cases, the motor resistance typically shows 200 Ω. Try again. Also, try to run a mission before the motortest. There must be some data-handling issues that are not adequately addressed; this issue is not fixed yet (Dec 2025).