# Control

# Overview

### General structure

The general structure of how the control system is made, tested, and deployed works like this:

<span style="white-space: pre-wrap;">The control system is made using Simulink, using mostly the base functionality to calculate the needed values for different parts of control. Once the general control system is implemented, it is either tested directly in Simulink, mostly using different parts from the Simscape suite to simulate the hardware, or it can be tested by generating the code, and using that code in a separate, more high level simulation program like Gazebo or Webots. </span>

The deployment is mostly managed by the embedded team, as once the code is generated, it can be used in whatever is necessary for the embedded team.

### Required software

MATLAB Simulink with the following add-ons:

- Embedded Coder
- Simulink Coder
- Simulink Compiler
- MATLAB Coder
- MATLAB Compiler

### Code generation

In general the code is generated from Simulink blocks, where all the control has to do is done inside this one block:

[![image.png](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/scaled-1680-/FNEimage.png)](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/FNEimage.png)

To be able to generate C code for the STM32's you will first need to activate the embedded C coder, which can be done by clicking on the apps tab in the top bar, and then searching for C embedded coder in the apps bar.

[![image.png](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/scaled-1680-/QUCimage.png)](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/QUCimage.png)

After the embedded C coder is active, a new tab called C CODE appears, where you now have to press the quick start button to start the code generation process:

[![image.png](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/scaled-1680-/DSwimage.png)](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/DSwimage.png)

When in the quick start menu, the only critical options for this project are to set the system which gets generated to the main control block of this project, and to set the word size to be the same as the ones from the STM32.

[![image.png](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/scaled-1680-/BR5image.png)](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/BR5image.png)

[![image.png](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/scaled-1680-/isOimage.png)](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/isOimage.png)

The rest of the settings work best on the default option, but can be changed if deemed necessary.

# Manual control



# Automatic control

### Overview

In general, the job of the control system is to turn high level instructions from the software system, and turn them into instructions for the hardware. This is done by having a distance to a goal, a turning radius, and a deltaTime as an input, and turning those into control signals for the motors:

[![image.png](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/scaled-1680-/Howimage.png)](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/Howimage.png)

[![image.png](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/scaled-1680-/3gBimage.png)](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/3gBimage.png)

This is done in several steps.

#### Determining current desired speed

To determine what speed the rover should move at this moment, the distance to the goal is used to decide how fast the rover should move, which in general means, closer to the goal = move slower, and if we are really close, stop moving completely. If the rover is not close to the goal, it will try to move at v0 speed.

[![image.png](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/scaled-1680-/J9Iimage.png)](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/J9Iimage.png)

The function simply works like this:

```matlab
function [v_out, atgoal] = speed(dist2goal, v0)

%slow down if close to goal
if dist2goal <= 1
    v_out = 0;
    atgoal = 1;

%stopping if the rover is close to the target
elseif dist2goal <= 2
    v_out = v0 * 0.5;
    atgoal = 0;

%keeping cruising speed if not close to goal
else
    v_out = v0;
    atgoal = 0;
end
```

#### Ackermann steering

Getting the necessary angles that the steering motors need to make is done by calculating the Ackermann steering angles that equate from the given turning radius and distance to the goal.

[![image.png](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/scaled-1680-/GvFimage.png)](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/GvFimage.png)

deltaL and deltaR are equal to the angles that the left and right steering motors need to make, which are sent to the steering control.

This function also outputs the turning radius of the right and left wheels of the rover.

The function works like this:

```matlab
function [deltaL,deltaR,R,wheelBase,R_left,R_right] = ackermann_angles(dist2goal,R,wheelBase,track)
    % Function to compute ackerman steering angles from a bicycle steer angle.
    if R == 0
        alpha = 0;
    else
        alpha = dist2goal/R;
    end
    
    R = abs(R);
    
    if alpha < 0
        R_left = (R+track/2);
        R_right = (R-track/2);
        deltaL = atan(wheelBase/(R_left));
        deltaR = atan(wheelBase/(R_right));
    elseif alpha > 0
        R_left = (R-track/2);
        R_right = (R+track/2);
        deltaL = -atan(wheelBase/(R_left));
        deltaR = -atan(wheelBase/(R_right));
    else
        deltaL = 0;
        deltaR = 0;
        R_left = R;
        R_right = R;
    end
end
```

#### Ackermann speeds

Using the data coming from the previous function, as well as a wheel radius and motor gear ratio, the speeds needed by the motors are calculated.

[![image.png](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/scaled-1680-/CCIimage.png)](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/CCIimage.png)

Internally the function works like this:

```matlab
function [wheel_speed_LF,wheel_speed_LM,wheel_speed_LB,wheel_speed_RF,wheel_speed_RM,wheel_speed_RB] = ...
          ackermann_speeds(desChassisSpeed,wheel_r,R,wheelBase,R_left,R_right,gearRatio)
if R > 0
    wheel_speed_LF = (desChassisSpeed/wheel_r)*sqrt(R_left^2 + (wheelBase/2)^2)/R;
    wheel_speed_LM = desChassisSpeed * R_left/(R*wheel_r);
    wheel_speed_LB = (desChassisSpeed/wheel_r)*sqrt(R_left^2 + (wheelBase/2)^2)/R;
    wheel_speed_RF = (desChassisSpeed/wheel_r)*sqrt(R_right^2 + (wheelBase/2)^2)/R;
    wheel_speed_RM = desChassisSpeed * R_right/(R*wheel_r);
    wheel_speed_RB = (desChassisSpeed/wheel_r)*sqrt(R_right^2 + (wheelBase/2)^2)/R;
else
    wheel_speed_LF = desChassisSpeed / wheel_r;
    wheel_speed_LM = desChassisSpeed / wheel_r;
    wheel_speed_LB = desChassisSpeed / wheel_r;
    wheel_speed_RF = desChassisSpeed / wheel_r;
    wheel_speed_RM = desChassisSpeed / wheel_r;
    wheel_speed_RB = desChassisSpeed / wheel_r;
end
    wheel_speed_LF = wheel_speed_LF * gearRatio;
    wheel_speed_LM = wheel_speed_LM * gearRatio;
    wheel_speed_LB = wheel_speed_LB * gearRatio;
    wheel_speed_RF = wheel_speed_RF * gearRatio;
    wheel_speed_RM = wheel_speed_RM * gearRatio;
    wheel_speed_RB = wheel_speed_RB * gearRatio;
end
```

#### Control signals

##### DC motors

After getting all the angles and speeds needed for the rover to make a certain movement, the signals get sent to the motor drivers/controllers in a way that they can use them.

[![image.png](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/scaled-1680-/m0Fimage.png)](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/m0Fimage.png)

The DC motors are controlled by first sending the desired speed into a linear interpolation function block, which just makes the desired speed change like a linear function, instead of as a stepping function gradually changing the speed to limit the amount of amps being pulled at startup.

```matlab
function out = linear_interpolation(current, desired, accelleration)
if current < desired
    current = current + accelleration;
end
if current > desired
    current = current - accelleration;
end

out = current;
```

After this, the linearly changing desired speed will be compared with the actual speed of the motors and the final speed will be controlled using PI controllers which are saturated to stay between -5V and 5V.

##### Stepper motors

Due to the limitations of Simulink, making the stepper motors move is done using PWM signals coming from the STM32 microcontrollers. The control system only gives the desired position of the stepper motors in terms of steps, and a frequency at which the PWM signal pulses, which in this case is just a constant value.

[![image.png](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/scaled-1680-/wAAimage.png)](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/wAAimage.png)

The sending of the PWM signals is handled by the embedded team.

# Simulation

## Simulation of the drive system

### Simulink

#### Overview

The simulation is done using a main control block called control\_drive, which as inputs gets a turning radius, a distance to goal, and a deltaTime. From these inputs, different control signals are calculated for the different motors, using simulated DC motors as well as stepper motors. Both the types of motors send the relevant feedback to the control block using goto blocks.

[![image.png](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/scaled-1680-/2ZVimage.png)](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/2ZVimage.png)

The driving motors are connected directly to the control block, processing the control signal directly.

The steering motors are connected to pulse generators to send instructions.

#### Driving motors

The driving motors are simulated as 6 motor blocks that have a control signal as input, and as outputs a speed from the motor, speed from the wheel, motor voltage and amperage.

[![image.png](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/scaled-1680-/image.png)](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/image.png)The driving motors are simulated with Simscape components, at first using a gain block with a 24/5 gain to what the motor driver would be doing (turning a 0V-5V signal to a 0V-24V signal). This value is sent to Voltage Source, which sends sends a voltage to a DC motor to create a rotational force.

[![image.png](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/scaled-1680-/lcnimage.png)](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/lcnimage.png)

This voltage is also read by a voltage and current sensor to get the expected power values at different speeds.

[![image.png](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/scaled-1680-/NgJimage.png)](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/NgJimage.png)

The speed of rotation is measured both before, and after the connection to the gearbox:

[![image.png](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/scaled-1680-/kOnimage.png)](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/kOnimage.png)

This is to get both the speed that would be read by the hall-sensors in the actual motors, as well as the speed going to the wheels to do further calculations with. The rotation of the motor after the gearbox is also connected to a tire with a mass equal to 1/6th of the mass of the rover.

Six of these motors are simulated, with the motor speed being used as feedback for the control system, and the wheel speed being used to calculate the actual speed of the rover.

#### Steering motors

The steering motors are connected to the control by connecting them to pulse generators that send signals to the stepper motors to set them to the correct positions.

[![image.png](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/scaled-1680-/gYgimage.png)](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/gYgimage.png)

The calculation before the current position enters the signal generator is used to convert the motor angle from radians to stepper motor steps. This is done to be able to control the stepper motors with the amount of steps that have been/ need to be taken to get to a certain position.

The stepper motor blocks themselves contain a simple stepper motor with a stepper motor driver, reading the final angle the motor moved to as output.

[![image.png](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/scaled-1680-/OqZimage.png)](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/OqZimage.png)

To control the motors, an ENA and REV signal are sent from pulse generator blocks (REV specifies the direction that the motor spins and ENA is a pulse signal activating the steps):

[![image.png](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/scaled-1680-/8Wximage.png)](https://bookstack.roboteamtwente.nl/uploads/images/gallery/2026-04/8Wximage.png)

This is done to simulate the behaviour of the PWM signals that will be sent by the STM32 microcontroller.

### Webots