Code: Select all
this.client.setStepping(true);
this.sim.startSimulation();
position_0 = this.sim.getObjectPosition(this.pioneer, this.sim.handle_world);
while 1
% t = this.sim.getSimulationTime();
position = this.sim.getObjectPosition(this.pioneer, this.sim.handle_world);
if abs(position{1}-position_0{1}) >= this.cell_length || abs(position{2}-position_0{2}) >= this.cell_length break; end
this.sim.setJointTargetVelocity(this.rightMotor, this.velocity_wheel); % en rad/s
this.sim.setJointTargetVelocity(this.leftMotor, this.velocity_wheel);
this.client.step();
end
while 1 % rotate 90º
orientation = this.sim.getObjectOrientation(this.pioneer, this.sim.handle_world);
if abs(orientation{3}-orientation_0{3}) >= 89.9*pi/180 && abs(orientation{3}-orientation_0{3}) <= 90.1*pi/180 break; end
this.sim.setJointTargetVelocity(this.rightMotor, this.velocity_turning); % en rad/s
this.sim.setJointTargetVelocity(this.leftMotor, -this.velocity_turning);
this.client.step();
end