[T CODE 0] Asservissement

Asservissement

Objective

Many factors can disturb(uneven surfaces, usury of the wheels...) the final position and angle where we want the robot to be. That's why we need some kind of "feedback" from the actual position of the robot. Which means that we tell it to move to coordinate XY, and then we measure the actual position (using encoders) to determine what adjustments we need to make. For this we defined an interruption that happens with a frequency of 200Hz.It is during this interruption that the position will be updated than the motor speed will be adjusted accordingly. To simplify the problem we will consider two different type of position orders:

1. An order in position, that ask the robot to go somewhere (to reduce the difficulty the robot should be already orientated toward the destination).
2. An order in angle that ask the robot to orientate to a given angle.

Requirements:

For the base of the robot we used 2 wheels located is the middle of the robot (linked to DC motors alimented throught a driving card) and two external coding wheels linked to 2 encoders. The encoder we used are AMT102. They are modular incremental encoder that are used when the the rotary movement. They are useful because we can change the precision of the measurement by changing the number of ticks per lap, to do it we need to use the switches located inside the encoder. The resolution can go from 48 to 2048 ticks. The coding wheels are separated from the wheels because the wheels may slip and then the measured position may be wrong. To control the encoder with the STM32 we need the the Quadrature Encoder Interface (QEI) module which provides the interface to incremental encoders for obtaining mechanical position data. We used the QEI library located is the files QEI.cpp and QEI.h. These files are taken from here, in open source. Finally, we need an STM32. The model we used is a NUCLEO-L476RG.

Steps:

The first thing to do in the asservissement in the odometry (calculating the position).

The code used to calculate the position is located inside "motionCtrl" and especially in the method "update_Pos()". The code is fairly short and is well explained here The equation to obtain the new position are below, θ_being the angle of the robot as seen in the picture.
Xt=X(t-1)+(cos⁡(θ(t-1)+TickWheelsLeft- TickWheelsRight)*(((TickWheelsLeft+TickWheelsRight))/2))

Yt=Y(t-1)+ +(sin⁡(θ(t-1)+TickWheelsLeft- TickWheelsRight)*(((TickWheelsLeft+TickWheelsRight))/2))

θt=θ(t-1)+(TickWheelsLeft- TickWheelsRight)

After calculating the position we will need to adjust the value given to the motors

1. First we need to calculate the distance between the current location and the location we want to go. Then we need to calculate the angle between the wheel axis and the line between our current position and the intended position as shown in the schema below. this calculation is done in motionCtrl in the method "DefineDistCap()". "Cap" is the angle and "Dist" is the distance. In the case of an order in position we consider Dist and cap in the calculus whereas in an order in angle we will only consider cap.

2. The second step is to calculate the speed we wish to reach. There are 3 phases during the movement of the robot:

• The first one is accelerating. During this phase the speed is below the speed limit that we decide to use. It is important that the motor accelerate in a linear and slow way to limit the chance of skidding. 4 Values have to be known. The period of the asservissement loop: Dt= 1/200, The robot speed Srobot, the max speed MaxS and the maximum acceleration MaxA. In accelerating phase, the consign is the following: Sconsign = Srobot + (MaxA x Dt)
• The second phase is the full speed phase. The robot is in this phase when it reached the max speed and the robot is not in baking phase. The Sconsign= MaxS.
• The third phase is the braking phase. During this phase the robot is close to his destination and needs to slow down and stop at the required position. We need to know the Max Deceleration: MaxD to calculate the BrakeDist: BrakeDist = (Srobot²) / (2 x MaxD). In braking phase, the consign is the following: Sconsign= Srobot – (MaxD x dt)
• The result is the following algithm:

If(dist< BrakeDist) Sconsign= Srobot – (MaxD x dt);

Else if (Srobot<MaxS) Sconsign= Srobot + (MaxA x dt);

Else Sconsign= MaxS;

3. Then we need to calculate the value to give to the motors. To do this we will use a proportional–integral–derivative controller (PID controller). It is a controller that is used to rectify an error between a desired value and the actual value. It output a correction to reach the value with a minimum of Overshoot, Steady-state error. This action is done in the method "Compute_PID()" to do that we use the "compute()" method located in the PID class. There is two different PID the one used to the influence of "Dist" in the value and the other to calculate the "Cap" value. We than cap the value between the min value of the PWM the motor can work with and the maximum one. After that we change the value of the motors.

Conclusion:

The position control is really important for the robot and should be a priority. Without it the robot can't do anything. It require a lot of testing to determine the Max speed, Max decel etc...

References:

cubot guide