def navigate(self): lastOrientationDegree = 0; turn_degrees_needed = 0; turn_degrees_accum = 0; imu = Imu(); #clean angle; imu.get_delta_theta(); #Condition distance more than 2 meters. while distance > 2 and self.stopNavigation != False: #print("degrees: ", imu.NORTH_YAW); #print("coords: ", imu.get_gps_coords()); #print("orientation degrees", orientationDegree); if(lastOrientationDegree != orientationDegree): turn_degrees_needed = orientationDegree; turn_degrees_accum = 0; #clean angle; imu.get_delta_theta(); lastOrientationDegree = orientationDegree; #If same direction, keep route #while math.fabs(turn_degrees_needed) > 10: imu_angle = imu.get_delta_theta()['z']%360; if(imu_angle > 180): imu_angle = imu_angle - 360; #print("grados imu: ", imu_angle); #threshold if(math.fabs(imu_angle) > 1): turn_degrees_accum += imu_angle; #print("grados acc: ", turn_degrees_accum); turn_degrees_needed = (orientationDegree + turn_degrees_accum)%360; if(turn_degrees_needed > 180): turn_degrees_needed = turn_degrees_needed - 360; elif (turn_degrees_needed < -180): turn_degrees_needed = turn_degrees_needed + 360; #print("grados a voltear: ", turn_degrees_needed); if(math.fabs(turn_degrees_needed) < 10): print("Tengo un margen menor a 10 grados"); velocity = destiny['distance'] * 10; if (velocity > 300): velocity = 200; motors.move(velocity, velocity); else: #girar if(turn_degrees_needed > 0): print("Going to move left") motors.move(70, -70); else: print("Going to move right") motors.move(-70, 70); #ir derecho; #recorrer 2 metros destiny = imu.get_degrees_and_distance_to_gps_coords(latitude2, longitud2); #time.sleep(1); motors.move(0,0); print("End thread Navigation");
def navigate(self, destiny, lat, lon): lastOrientationDegree = 0 turn_degrees_needed = 0 turn_degrees_accum = 0 distance = destiny['distance'] orientationDegree = destiny['degree'] imu = Imu() #clean angle imu.get_delta_theta() #Condition distance more than 2 meters. while distance > 2 and self.stopNavigation != False: #print("degrees: ", imu.NORTH_YAW) #print("coords: ", imu.get_gps_coords()) #print("orientation degrees", orientationDegree) if lastOrientationDegree != orientationDegree: turn_degrees_needed = orientationDegree turn_degrees_accum = 0 #clean angle imu.get_delta_theta() lastOrientationDegree = orientationDegree #If same direction, keep route #while math.fabs(turn_degrees_needed) > 10: imu_angle = imu.get_delta_theta()['z'] % 360 if imu_angle > 180: imu_angle = imu_angle - 360 #print("grados imu: ", imu_angle) #threshold if math.fabs(imu_angle) > 1: turn_degrees_accum += imu_angle #print("grados acc: ", turn_degrees_accum) turn_degrees_needed = (orientationDegree + turn_degrees_accum) % 360 if turn_degrees_needed > 180: turn_degrees_needed = turn_degrees_needed - 360 elif turn_degrees_needed < -180: turn_degrees_needed = turn_degrees_needed + 360 #print("grados a voltear: ", turn_degrees_needed) if math.fabs(turn_degrees_needed) < 10: print("Tengo un margen menor a 10 grados") velocity = distance * 10 if velocity > 250: velocity = 200 motors.move(velocity, velocity) else: #girar if turn_degrees_needed > 0: print("Going to move left") motors.move(70, -70) else: print("Going to move right") motors.move(-70, 70) #ir derecho #recorrer 2 metros destiny = imu.get_degrees_and_distance_to_gps_coords(lat, lon) #time.sleep(1) motors.move(0, 0)