Commit 0e888a8e authored by Mugisha David's avatar Mugisha David

Simple test before implementing wheel control

parent 5c846cee
......@@ -41,13 +41,13 @@ class RoverKinematics:
# Insert here the steering and velocity of
# each wheel in skid-steer mode
motors.steering[k] = twist.angular.z
motors.drive[k] = twist.linear.x
motors.drive[k] = 2*twist.linear.x
else:
for k in drive_cfg.keys():
# Insert here the steering and velocity of
# each wheel in rolling-without-slipping mode
motors.steering[k] = 0
motors.drive[k] = 0
motors.steering[k] = twist.angular.z
motors.drive[k] = 2*twist.linear.x
return motors
def integrate_odometry(self, motor_state, drive_cfg):
......@@ -57,9 +57,11 @@ class RoverKinematics:
self.first_run = False
return self.X
# Insert here your odometry code
self.X[0,0] += 0.0
self.X[1,0] += 0.0
self.X[2,0] += 0.0
self.X[0,0] += (drive_cfg['FR'].x + drive_cfg['FL'].x + drive_cfg['RL'].x + drive_cfg['RR'].x)/4
self.X[1,0] += (drive_cfg['FR'].y + drive_cfg['FL'].y + drive_cfg['RL'].y + drive_cfg['RR'].y)/4
self.X[2,0] += (drive_cfg['FR'].radius + drive_cfg['FL'].radius + drive_cfg['RL'].radius + drive_cfg['RR'].radius)/4
self.motor_state.copy(motor_state)
return self.X
......
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