Chapter 13 Kinematic Analysis

In this chapter, we will get to know the speciality of omni wheels and how an omni-wheeled car can move in any direction.

Circuit

For this chapter, we use the assembled car. Please refer to Chapter 1 Car Assembly for the detailed assembly process.

../../../_images/Chapter13_14.png

Sketch

Next, we download the code to Raspberry Pi Pico (W) to examine the attitude calculation. Open “Sketch_11.1_Kinematic_analysis” folder under “Freenove_Omni_Wheel_Car_Kit_for_Raspberry_Pi_Pico\Four-Wheel\Sketches” and double-click “Sketch_11.1_Kinematic_analysis.ino”.

../../../_images/Chapter13_07.png

Code

Sketch_11.1_Kinematic_analysis.ino

 1/***********************************************************************************
 2  Filename    : Sketch_11.1_Kinematic_analysis
 3  Description : Use Raspberry Pi Pico Control the trolley to move in all directions
 4  Auther      : www.freenove.com
 5  Modification: 2024/08/23
 6************************************************************************************/
 7
 8const int wheel1_A_pin = 8; // Motor1 drive pin
 9const int wheel1_B_pin = 9; // Motor1 drive pin
10
11const int wheel2_A_pin = 12;// Motor2 drive pin
12const int wheel2_B_pin = 13;// Motor2 drive pin
13
14const int wheel3_A_pin = 15;// Motor3 drive pin
15const int wheel3_B_pin = 14;// Motor3 drive pin
16
17const int wheel4_A_pin = 20;// Motor4 drive pin
18const int wheel4_B_pin = 21;// Motor4 drive pin
19
20int a1,b1,a2,b2,a3,b3,a4,b4;
21int v1,v2,v3,v4;
22
23void setup() {
24  // put your setup code here, to run once:
25  Motor_init();
26}
27
28void loop() {
29  // put your main code here, to run repeatedly:
30  Motor_Control(200,0);    // Forward
31  delay(1000);          
32  Motor_Control(-200,0);   // Back
33  delay(1000);
34  Motor_Control(200,90);   // Left translation
35  delay(1000);
36  Motor_Control(200,-90);  // Right translation
37  delay(1000);
38  Motor_Control(255,45);   // Shift 45 degrees to the left
39  delay(1000);
40  Motor_Control(255,-135); // Shift 135 degrees to the right
41  delay(1000);
42  Motor_Control(255,135);  // Shift 135 degrees to the left
43  delay(1000);
44  Motor_Control(255,-45);  // Shift 45 degrees to the right
45  delay(5000);
46
47}
48void Motor_Control(int speed_v,int speed_a)
49{
50  int vx =  - speed_v * sin ( speed_a * PI / 180 );
51  int vy =    speed_v * cos ( speed_a * PI / 180 );
52
53  v1 = -vx;
54  v2 = -vy;
55  v3 =  vx;
56  v4 =  vy;
57
58  Motor_direction();//Motor limiting and motor output orientation
59}
60
61void Motor_direction()
62{
63  if(v1 > 0){ a1 = v1; b1 = LOW; }else{ a1 = LOW; b1 = -v1;}
64  if(v2 > 0){ a2 = v2; b2 = LOW; }else{ a2 = LOW; b2 = -v2;}
65  if(v3 > 0){ a3 = v3; b3 = LOW; }else{ a3 = LOW; b3 = -v3;}
66  if(v4 > 0){ a4 = v4; b4 = LOW; }else{ a4 = LOW; b4 = -v4;}
67
68  analogWrite(wheel1_A_pin,a1);
69  analogWrite(wheel1_B_pin,b1);
70
71  analogWrite(wheel2_A_pin,a2);
72  analogWrite(wheel2_B_pin,b2);
73
74  analogWrite(wheel3_A_pin,a3);
75  analogWrite(wheel3_B_pin,b3);
76
77  analogWrite(wheel4_A_pin,a4);
78  analogWrite(wheel4_B_pin,b4);
79}
80
81void Motor_init()
82{
83  // Set the motor PWM frequency to 16 kHz
84  analogWriteFreq(16000);
85  // Enable the motor drive pin to output mode
86  pinMode(wheel1_A_pin,OUTPUT); 
87  pinMode(wheel1_B_pin,OUTPUT);
88  pinMode(wheel2_A_pin,OUTPUT);
89  pinMode(wheel2_B_pin,OUTPUT);
90  pinMode(wheel3_A_pin,OUTPUT);
91  pinMode(wheel3_B_pin,OUTPUT);
92  pinMode(wheel4_A_pin,OUTPUT);
93  pinMode(wheel4_B_pin,OUTPUT);
94}

After downloading the code, put the car on the floor, turn ON the power switch of the car, and you will see to car sequentially perform the actions of moving forward, moving backward, moving left, moving right, moving at a 45-degree angle to the left, moving at a 45-degree angle to the right, moving at a 135-degree angle to the left, and moving at a 135-degree angle to the right.

Code Explanation

When the Motor_init() function is called, pico enables the motors’ pins to output mode.

 1void Motor_init()
 2{
 3  // Set the motor PWM frequency to 16 kHz
 4  analogWriteFreq(16000);
 5  // Enable the motor drive pin to output mode
 6  pinMode(wheel1_A_pin,OUTPUT); 
 7  pinMode(wheel1_B_pin,OUTPUT);
 8  pinMode(wheel2_A_pin,OUTPUT);
 9  pinMode(wheel2_B_pin,OUTPUT);
10  pinMode(wheel3_A_pin,OUTPUT);
11  pinMode(wheel3_B_pin,OUTPUT);
12  pinMode(wheel4_A_pin,OUTPUT);
13  pinMode(wheel4_B_pin,OUTPUT);
14}

The function Motor_Control() calculates the velocity of each wheel.

 1void Motor_Control(int speed_v,int speed_a)
 2{
 3  int vx =  - speed_v * sin ( speed_a * PI / 180 );
 4  int vy =    speed_v * cos ( speed_a * PI / 180 );
 5
 6  v1 = -vx;
 7  v2 = -vy;
 8  v3 =  vx;
 9  v4 =  vy;
10
11  Motor_direction();//Motor limiting and motor output orientation
12}

The function Motor_Control() is called under the loop function to control the car’s movement.

1Motor_Control(255,135);  // Shift 135 degrees to the left

Reference

double sqrt(double x);

This function is used to calculate the square root of a number.

void Motor_Control (int speed_v,int speed_a);

The function is used to control the movement of the car.

speed_v: the speed of the car’s translation, ranging from -255 to 255.

speed_a: the direction of the car’s movement, in degrees, ranging from -180 to 180.