Regbot firmware

Back to Regbot

Main structure

The main (Arduino) file is regbot.ino

Arduino starts by calling a setup() function. This function sets up all modules and used libraries. All calibration values and other saved settings are loaded and implemented during the setup.

Arduino then calls loop(). This function holds the main loop, doing:

• Listen for incoming commands (from USB) and service data subscriptions.
• At regular intervals (1ms):
  • Read sensors,
  • Run control
  • Implement on actuators
  • Do data logging

Module list

Modules are encapsulated in a class. They are all defined in a header file (*.h), which includes the instance's global name. The implementation is a *.cpp file with the same name.

All modules, sub-modules and some additional library support files are in the src subdirectory.

The modules and sub-modules are (in alphabetic order):

• uad (interrupt based Analogue to digital conversion.)
• ucommand (A service module handling a few commands that didn't fit anywhere else.)
• ucontrol (Implements all control actions, then is both PID controllers and the connection between them.)
  • ucontrolbase (The base elements of a PID controller, i.e. an integrator and a pole-zero filter, and combine these to a PID controller.)
  • ucontrolturn (A specialized controller for turning. An extension of the control base.)
• ucurrent (Current measurements calculations.)
• udisplay (Handling of the small 128x32 pixel O-Led display.)
• ueeconfig (Handling of save and recall configuration from flash memory.)
• uencoder (Calculating robot pose and velocity based on motor encoder values.)
• uimu2 (Calculation tilt based on accelerometer and gyro.)
• uirdist (Calculation distance based on analogue values from sensor, as well as sensor on and off)
• ulinesensor (Line sensor calibration and data handling.)
• ulog (Data logging.)
• umission (Hold and maintain all mission lines.)
  • umissionline (Mission line class.)
  • umissionthread (Mission thread handling.)
• umotor (Motor driver interface.)
• umotortest (Support function that attempts to estimate motor parameters.)
• urobot (Handling of the robot such as battery monitoring, power off and status LED.)
• uservo (interface to servos.)
• uusb (Handling interface on USB, including data subscriptions.)
  • usubs (Subscription item class.)
  • usubss (Subscription support class for all modules.)
• uusbhost (Support for gamepad control of the robot using the USB-host connector (if connected).)

Pin definitions are (mostly) in the main.h file.

All files not starting with the letter u are functions found on the net or modified library functions.

Module class structure

The structure of a module has this base structure:

class URobot : public USubss
{
public:
  void setup();
  void sendHelp() override;
  bool decode(const char * buf) override;  
  void tick();
  void sendData(int item) override;
  void eePromLoad();
  void eePromSave();
}

Inheritance

All module classes inherit from the class usubss. This enables handling of:

• incomming commands by the decode() function.
• on-line help (sendHelp().)
• publishing of subscribed data by sendData(int item).

These functions related to USB, are handled by the uusb module.

Tick()

This function is called at every sample interval and service all dynamics of the module.

eePromSave() eePromLoad()

These functions save and load the configuration and calibration data to the flash memory (electronically erasable memory)

Mission control

The mission lines set variables that govern the PID controllers.

The yellow blocks are the PID controllers. Light green is calculated values, and dark green represents sensor values.

Forward velocity can be set by:

• mission line value,
• the balance controller, or
• the position control.

Turning can be set from:

• mission turn radius,
• the heading control,
• the line edge control, or
• the wall distance control.

The active mission line determines which to use.

Mission start

Missions start is handled by the tick() function in the ucontrol module. It also handled hard-coded missions and remote control, but the important function is:

• the call to used_mission()

User mission

The userMission() handles activation and termination of missions using states:

• state 0:
  • test the robot.missionStart flag
  • initializes control
  • turns on distance sensor and line sensor, if needed by any mission line.
• state 1:
  • Turns off display updates (may in some cases take more than 1ms)
  • sets PID control to active.
• state 2:
  • Check if the mission or mission line is finished (also advances mission line as needed).
  • Check if the stop button is pressed.
  • Control stays in state 2 as long as the mission is running.
• state 8:
  • Stop mission and motors.
  • Return to state 0

PID controller configuration

The setup() in the ucontrol module calls a function called setRegulatorInOut(). This function defines the three values needed by all controllers:

• reference or setpoint.
• measured value in the same units as the reference value.
• output value of the controller.

All PID parameters are saved in configuration flash memory and are best set using the GUI.

Look in ucontrolbase.cpp line 410 to see how the control parameters are decoded from a USB message. The format is the same for all PID controllers, and here exemplified for the balance controller:

   // cbal 1 -3 0 1 9999.99 1 0 1 1 1 0.029 0.001 0 1 1 1 0.1 5 1 0 1 0 1 1 1 99.99
   // 1 cbal 1 // use (any part of controller)
   // 2     -3 // Kp
   // 3      0 1 9999.99 1 // integrator (use, tau, limit, (and_zero - not implemented in this case))
   // 7      0 1 1 // lead forward (use, zero, pole)
   // 10     1 0.029 0.001 // lead backward (use, zero, pole)
   // 13     0 1 1 // pre-filt (use, zero, pole)
   // 16     1 0.1 5 1 // post integrator (use, tau, limit, and_zero)
   // 20     0 1 // feed forward (use, Kff)
   // 22     0 1 1 // feed forward pole-zero (use zero pole)
   // 25     1 99.99 // output limit (use, limit)

For the balance controller td is used as the derivative gain and the y-axis gyro is used as 'm-dot' input. For all other controllers, the 'm-dot' input is not used.

PID controller in and out

All PID controllers are owned by the control module, and take three parameters with these values. One exception is the balance controller, where the gyro can be used as the derivative value of the tilt angle.

These connections should not be changed, but show how they are interconnected.

The setRegulatorInOut() function looks like this (ucontrol.cpp around line 525):

void UControl::setRegulatorInOut()
{ // set initial input and output for regulators
  // turn controllers
  ctrlWallVel->setInputOutput(&mission_wall_vel_ref, irdist.irDistance, &ctrl_vel_ref);
  ctrlWallTurn->setInputOutput(&mission_wall_ref, irdist.irDistance, vel_ref_red_turn, 0.3);
  // left and right line valid is now one value only
  ctrlEdge->setInputOutput(&mission_line_ref, &ls.lsLeftSide, &ls.lsRightSide, &ls.lsEdgeValid/*, &lseRightValid*/, 
                           vel_ref_red_turn, &mission_line_LeftEdge);
  ctrlTurn->setInputOutput(&ctrl_turn_ref /* &mission_turn_ref*/, &encoder.pose[2], vel_ref_red_turn);
  // position controller
  ctrlPos->setInputOutput(&mission_pos_ref, &encoder.distance, &misStartDist, &ctrl_vel_ref);
  // balance controllers
  ctrlBalPos->setInputOutput(&mission_pos_ref, &encoder.distance, &misStartDist, &ctrl_vel_ref);
  ctrlBalVel->setInputOutput(&ctrl_vel_ref, encoder.wheelVelocityEst, &balanceTiltRef);
  ctrlBal->setInputOutput(&balanceTiltRef, &encoder.pose[3], &balanceVelRef, &imu2.gyroTiltRate);
  // wheel velocity control
  ctrlVelLeft->setInputOutput(&vel_ref[0], &encoder.wheelVelocityEst[0], &motor.motorVoltage[0]);
  // wait with implementing motor voltage limit, to allow turning at all times
  // see tick on how this is implemented
  ctrlVelLeft->setDoImplementOutputLimitFlag(false);
  ctrlVelRight->setInputOutput(&vel_ref[1], &encoder.wheelVelocityEst[1], &motor.motorVoltage[1]);
  ctrlVelRight->setDoImplementOutputLimitFlag(false);
}

Control without mission

Control function takes a few input values (like desired velocity) and ends by setting the left and right motor voltage.

Activate contrl

To use the control functions without a mission requires:

• call control.resetControl(); to reset stored values (like integrators)
• set control.controlActive = true;. This will activate all control functions on tick() calls.

Desired velocity

Setting the variable control.mission_vel_ref (m/sec) will set the forward velocity.

The control.balance_active determines who sets the desired wheel velocity. If true, then the source is the balance controller. The control.mission_vel_ref is then the reference for the in-balance velocity controller.

Desired position

If control.mission_pos_use is true, then position control is active, and this PID controller sets the velocity (both in balance or not mode). The control.mission_pos_ref is used directly for position control.

To use distance control, the reference position must be set in control.misStartDist and the measured position is in encoder.distance.

In this case, the control.mission_vel_ref is used as the maximum allowed velocity.

Turning

Turning can be set by:

• Turn radius. This mode calculates the desired left and right wheel velocity.
  • set control.mission_turn_radius in meters and always positive, and set control.mission_tr_turn to a positive value to turn left.
  • Turn radius is used if control.mission_turn_do is set to true.
• Desired heading. Uses the heading controller to keep a desired heading (or turn to a desired heading).
  • set control.ctrl_turn_ref to desired value in degrees.
  • Requires that control.mission_turn_do is false (and also control.regul_line_use and control.mission_wall_turn both are false).
• Line sensor or wall distance sensor can also be used.