Motion Control

Motion Control

The motion class in SSL firmware works as a communication interface with the motors that allows abstract from the main code the vector cinematics of the navigation class to calculate the speed of each wheel and a control and feedback system to guarantee the execution of the vector movement sent by the high level.

From the high level, the robot receives the vector velocities on the Cartesian axes Vx e Vy in m/s and rotation W in rad/s, this data is sent to the motion object which internally performs the following steps.

1. Send these vectors to navigation that is based on the robot's movement possibilities (in SSL it is omnidirectional), on the dimensions and angles of the motors to calculate the speed required in each motor so that the sum of these vectors is the movement requested by the top level.

• Calculation for each wheel:

𝜎1 = (sin(M1_Angle)/WheelR) * Vx + (cos(M1_Angle)/WheelR) * Vy + (RobotR/WheelR) * W
𝜎2 = (sin(M2_Angle)/WheelR) * Vx + (-cos(M2_Angle)/WheelR) * Vy + (RobotR/WheelR) * W
𝜎3 = (-sin(M3_Angle)/WheelR) * Vx + (-cos(M3_Angle)/WheelR) * Vy + (RobotR/WheelR) * W
𝜎4 = (-sin(M4_Angle)/WheelR) * Vx + (cos(M4_Angle)/WheelR) * Vy + (RobotR/WheelR) * W

This decomposed information for each engine is received by motion and the desired speed for each engine is forwarded to the corresponding PIDController class instance.

2. Each PIDController instance has two objects one of the DriverBLDC class that represents the direct control/communication interface with the motor and another one of the encoder class that by default is built based on timer encoder but can also be built to be based at interruptions, it represents the motor speed sensor that provides the current reading for the motor.

   At each run The PIDController will read the current speed of the motor and based on the value read from the encoder and the desired speed it will calculate the associated error proportion and the errors accumulated in previous runs to define the new engine speed setpoint which will be converted to PWM and sent to the motor by DriverBLDC.

• Because of this control logic it is necessary to constantly execute the methods of the motion object.