Skip to content

Commit fa6f91d

Browse files
Abhinav-PrajapatiAbhinav-Prajapati
committed
Change Drive config form X to varient of +
1 parent 4582bd2 commit fa6f91d

File tree

2 files changed

+91
-47
lines changed

2 files changed

+91
-47
lines changed

‎r2/ik_test.py

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
import numpy as np
2+
3+
def cmd_vel_callback():
4+
linear_x = 0
5+
linear_y = 0
6+
angular_z = 1
7+
matrix_4x3 = np.array([[15.75, 0, -5.66909078166105],
8+
[0, 15.75, 5.66909078166105],
9+
[-15.75, 0, 5.66909078166105],
10+
[0, -15.75,-5.66909078166105]])
11+
12+
13+
# Define a Bot Velocity ~ 3x1 matrix
14+
matrix_3x1 = np.array([[linear_x],
15+
[linear_y],
16+
[angular_z]])
17+
# Perform matrix multiplication
18+
result_matrix = np.dot(matrix_4x3, matrix_3x1)
19+
print(result_matrix)
20+
21+
22+
cmd_vel_callback()

‎r2/uart.py

Lines changed: 69 additions & 47 deletions
Original file line numberDiff line numberDiff line change
@@ -6,81 +6,70 @@
66
import math
77
from threading import Lock
88

9+
colors = {
10+
'red': '\033[91m',
11+
'green': '\033[92m',
12+
'yellow': '\033[93m',
13+
'blue': '\033[94m',
14+
'magenta': '\033[95m',
15+
'cyan': '\033[96m',
16+
'white': '\033[97m',
17+
'reset': '\033[0m'
18+
}
19+
920
class Uart(Node):
1021
def __init__(self):
1122
super().__init__('uart_node')
1223

13-
self.ROBOT_WIDTH = 0.229
14-
self.ROBOT_LENGTH = 0.550
15-
self.WHEEL_RADIUS = 0.127
16-
self.WHEEL_DIS_FROM_CENTER = 0.229
17-
self.WHEEL_GEOMETRY = 0.550 + 0.229 # Height and width in meters
18-
1924
self.pico_pwm = 254
20-
self.max_pwm = 20 / 100 * self.pico_pwm
25+
self.max_pwm = 20 / 100 * self.pico_pwm / 16
2126

2227
self.loop_rate=0.020 # miliseconds
2328
self.MotorPWM:[int,int,int,int]=[0,0,0,0]
2429

30+
self.matrix_4x3 = np.array([[15.75, 0, -5.66909078166105],
31+
[0, 15.75, 5.66909078166105],
32+
[-15.75, 0, 5.66909078166105],
33+
[0, -15.75,-5.66909078166105]])
2534

2635
self.mutex = Lock()
27-
self.ser = serial.Serial('/dev/ttyACM0', 115200)
36+
#self.ser = serial.Serial('/dev/ttyACM0', 115200)
2837
self.uart_timer=self.create_timer(self.loop_rate,self.uart_timer_callback)
2938
self.subscription = self.create_subscription(Twist,'piRobot/cmd_vel',self.twist_callback,10)
30-
# self.encoder=self.create_publis++++++++++++++++++++++++++++++++++++++++her(Motor_encoder,'/drive/raw_encoder',10)
39+
# self.encoder=self.create_publisher(Motor_encoder,'/drive/raw_encoder',10)
40+
self.get_logger().info(f"{colors['green']} Uart Node started ")
3141

3242

3343
def twist_callback(self, msg):
34-
Vx = msg.linear.x
35-
# Vy = msg.linear.y
36-
Vy = msg.angular.z
37-
# omega = msg.angular.z
38-
omega = msg.linear.y
39-
40-
self.MotorPWM[0] = (Vx - Vy - self.WHEEL_GEOMETRY * omega) / self.WHEEL_RADIUS
41-
42-
self.MotorPWM[1] = (Vx + Vy + self.WHEEL_GEOMETRY * omega) / self.WHEEL_RADIUS
43-
44-
self.MotorPWM[2] = (Vx + Vy - self.WHEEL_GEOMETRY * omega) / self.WHEEL_RADIUS
45-
46-
self.MotorPWM[3] = (Vx - Vy + self.WHEEL_GEOMETRY * omega) / self.WHEEL_RADIUS
47-
4844

49-
# self.MotorPWM[0] = (Vx - Vy - self.WHEEL_DIS_FROM_CENTER * omega) / self.WHEEL_RADIUS # Forward left
50-
51-
# self.MotorPWM[1] = (Vx + Vy + self.WHEEL_DIS_FROM_CENTER * omega) / self.WHEEL_RADIUS # Forward right
52-
53-
# self.MotorPWM[2] = (Vx + Vy - self.WHEEL_DIS_FROM_CENTER * omega) / self.WHEEL_RADIUS # backward left
54-
55-
# self.MotorPWM[3] = (Vx - Vy + self.WHEEL_DIS_FROM_CENTER * omega) / self.WHEEL_RADIUS # backward right
56-
57-
self.MotorPWM[0] = -int(self.MotorPWM[0] / 7.87 * self.max_pwm)
58-
self.MotorPWM[1] = -int(self.MotorPWM[1] / 7.87 * self.max_pwm)
59-
self.MotorPWM[2] = int(self.MotorPWM[2] / 7.87 * self.max_pwm)
60-
self.MotorPWM[3] = int(self.MotorPWM[3] / 7.87 * self.max_pwm)
45+
linear_x = msg.linear.x
46+
linear_y = msg.linear.y
47+
angular_z = msg.angular.z
48+
49+
# Define a Bot Velocity ~ 3x1 matrix
50+
matrix_3x1 = np.array([[linear_x],
51+
[linear_y],
52+
[angular_z]])
53+
# Perform matrix multiplication
54+
result_matrix = np.dot(self.matrix_4x3, matrix_3x1)
55+
56+
self.MotorPWM[0] = int(result_matrix[0] * self.max_pwm)
57+
self.MotorPWM[1] = int(result_matrix[1] * self.max_pwm)
58+
self.MotorPWM[2] = int(result_matrix[2] * self.max_pwm)
59+
self.MotorPWM[3] = int(result_matrix[3] * self.max_pwm)
6160

62-
self.get_logger().info('Motor PWM values: Motor1={}, Motor2={}, Motor3={}, Motor4={}'.format(self.MotorPWM[0], self.MotorPWM[1], self.MotorPWM[2], self.MotorPWM[3]))
61+
self.get_logger().info('Motor PWM values: {} Motor1={}, Motor2={}, Motor3={}, Motor4={}'.format(colors['yellow'],self.MotorPWM[0], self.MotorPWM[1], self.MotorPWM[2], self.MotorPWM[3]))
6362

6463
def uart_timer_callback(self):
6564

6665
self.mutex.acquire()
6766

6867
try:
69-
7068
dataToSend = f"{self.MotorPWM[0]},{self.MotorPWM[1]},{self.MotorPWM[2]},{self.MotorPWM[3]},\n".encode('utf-8')
7169
self.ser.write('m'.encode('utf-8'))
7270
self.ser.write(dataToSend)
7371

74-
# response = self.ser.readline().strip()
75-
# self.get_logger().info(f"sent {dataToSend} rec {response}")
76-
# response=0
77-
7872
self.ser.write('e'.encode('utf-8'))
79-
# response = self.ser.readline().strip() # read(3) will read 3 bytes
80-
# print(f"revived {response}")
81-
82-
# todo: publish encoder data here
83-
# encoder_data:[int,int,int]=[int(num) for num in response.split()]
8473

8574
finally:
8675
self.mutex.release()
@@ -92,4 +81,37 @@ def main(args=None):
9281
rclpy.shutdown()
9382

9483
if __name__ == '__main__':
95-
main()
84+
main()
85+
86+
87+
# Archived Lines
88+
89+
# Vx = msg.linear.x
90+
# # Vy = msg.linear.y
91+
# Vy = msg.angular.z
92+
# # omega = msg.angular.z
93+
# omega = msg.linear.y
94+
95+
# self.MotorPWM[0] = (Vx - Vy - self.WHEEL_GEOMETRY * omega) / self.WHEEL_RADIUS
96+
# self.MotorPWM[1] = (Vx + Vy + self.WHEEL_GEOMETRY * omega) / self.WHEEL_RADIUS
97+
# self.MotorPWM[2] = (Vx + Vy - self.WHEEL_GEOMETRY * omega) / self.WHEEL_RADIUS
98+
# self.MotorPWM[3] = (Vx - Vy + self.WHEEL_GEOMETRY * omega) / self.WHEEL_RADIUS
99+
100+
'''
101+
self.ROBOT_WIDTH = 0.229
102+
self.ROBOT_LENGTH = 0.550
103+
self.WHEEL_RADIUS = 0.127
104+
self.WHEEL_DIS_FROM_CENTER = 0.229
105+
self.WHEEL_GEOMETRY = 0.550 + 0.229 # Height and width in meters
106+
'''
107+
108+
# response = self.ser.readline().strip()
109+
# self.get_logger().info(f"sent {dataToSend} rec {response}")
110+
# response=0
111+
112+
113+
# response = self.ser.readline().strip() # read(3) will read 3 bytes
114+
# print(f"revived {response}")
115+
116+
# todo: publish encoder data here
117+
# encoder_data:[int,int,int]=[int(num) for num in response.split()]

0 commit comments

Comments
 (0)