Commit 7f53dbd0 authored by Mugisha David's avatar Mugisha David

rough implt

parent 0e888a8e
......@@ -34,8 +34,33 @@ class RoverKinematics:
self.motor_state = RoverMotors()
self.first_run = True
def speed_steering_distribution(self, twist):
vx_r = twist.linear.x
vy_r = twist.linear.y + 10*twist.angular.z
vx_c = vx_r
vy_c = twist.linear.y + 4*twist.angular.z
vx_f = vx_r
vy_f = twist.linear.y - 4*twist.angular.z
data = {'steering':{}, 'speed':{}}
data['steering']['RL'] = atan2(vy_r,vx_r)
data['steering']['RR'] = atan2(vy_r,vx_r)
data['steering']['CL'] = atan2(vy_c,vx_c)
data['steering']['CR'] = atan2(vy_c,vx_c)
data['steering']['FL'] = atan2(vy_f,vx_f)
data['steering']['FR'] = atan2(vy_f,vx_f)
data['speed']['RL'] = 10*hypot(vy_r,vx_r)
data['speed']['RR'] = 10*hypot(vy_r,vx_r)
data['speed']['CL'] = 10*hypot(vy_c,vx_c)
data['speed']['CR'] = 10*hypot(vy_c,vx_c)
data['speed']['FL'] = 10*hypot(vy_f,vx_f)
data['speed']['FR'] = 10*hypot(vy_f,vx_f)
return data
def twist_to_motors(self, twist, drive_cfg, skidsteer=False):
motors = RoverMotors()
comm = self.speed_steering_distribution(twist)
if skidsteer:
for k in drive_cfg.keys():
# Insert here the steering and velocity of
......@@ -46,8 +71,8 @@ class RoverKinematics:
for k in drive_cfg.keys():
# Insert here the steering and velocity of
# each wheel in rolling-without-slipping mode
motors.steering[k] = twist.angular.z
motors.drive[k] = 2*twist.linear.x
motors.steering[k] = comm['steering'][k]
motors.drive[k] = comm['speed'][k]
return motors
def integrate_odometry(self, motor_state, drive_cfg):
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment