def turn_to_angle( gyro, target_angle):
error = target_angle - gyro.angle()
while ( abs(error) >= 4):
adj_angular_speed = error * 1.5
robot.drive(0, adj_angular_speed)
wait(100)
error=target_angle - gyro.angle()
robot.stop(stop_type=Stop.BRAKE)
def move_straight_target_direction_motor_angle(gyro, distance_mm, speed_mm_s, target_angle):
turn_to_angle( gyro, target_angle)
# Use a different variable name for gyro so you dont get confused
gyro_target_angle = target_angle
left_motor.reset_angle(0)
motor_target_angle = int(DEGREES_PER_MM * distance_mm)
while (abs(left_motor.angle()) < abs(motor_target_angle)):
error = target_angle - gyro.angle()
log_string('Gyro :' + str(gyro.angle()) + ' err: '+ str(error))
adj_angular_speed = error * 1.5
robot.drive(speed_mm_s, adj_angular_speed)
wait(50)
robot.stop(stop_type=Stop.BRAKE)
def move_straight_target_direction(gyro, distance_mm, speed_mm_s, target_angle):
turn_to_angle( gyro, target_angle)
duration = abs(int(1000 * distance_mm / speed_mm_s))
time_spent=0
while (time_spent<duration):
error = target_angle - gyro.angle()
adj_angular_speed = error * 1.5
robot.drive(speed_mm_s, adj_angular_speed)
wait(200)
time_spent = time_spent + 200
robot.stop(stop_type=Stop.BRAKE)
def calibrate_gyro(new_angle=0):
current_speed=gyro.speed()
current_angle=gyro.angle()
wait(60)
gyro.reset_angle(new_angle)
wait(20)
from robot_setup import right_motor
from robot_setup import robot
from robot_setup import rack_motor
from robot_setup import crane_motor
from robot_setup import gyro
from robot_setup import touch_sensor
from robot_setup import color_sensor_left
from robot_setup import color_sensor_right
from robot_setup import color_sensor_center
from robot_setup import touch_sensor
from robot_setup import SOUND_VOLUME
from robot_setup import WHEEL_DIAMETER_MM
from robot_setup import AXLE_TRACK_MM
from robot_setup import SENSOR_TO_AXLE
from robot_setup import WHEEL_CIRCUM_MM
from robot_setup import DEGREES_PER_MM
# Get wheel circumference
WHEEL_CIRCUM_MM = 3.149 * 89
# 360 degrees -> WHEEL_CIRCUM_MM so 1 degree -> ?
DEGREES_PER_MM = 360 / WHEEL_CIRCUM_MM
motor_target_angle = int(DEGREES_PER_MM * distance_mm)
left_motor.reset_angle(0)
while (abs(left_motor.angle()) < abs(motor_target_angle)):
error = target_angle - gyro.angle()
robot.drive(speed_mm_s, error)
Stop.BRAKE
import bus_service_1
import bus_service_2
import stepcounter
import treadmill
import row
import flip
import weight
import slide
import bench
import basket
import utime
shared_all.calibrate_gyro(0)
time1 = utime.ticks_ms()
for x in range(10):
gyro.angle()
time2 = utime.ticks_ms()
for x in range(10):
a = time1 + 1
time3 = utime.ticks_ms()
for x in range(10):
a = color_sensor_center.color()
time4 = utime.ticks_ms()
for x in range(10):
a = color_sensor_center.reflection()
time5 = utime.ticks_ms()
for x in range(10):
shared_all.log_string('Log it s ' + str(time5) + ' ' + str(time4))
time6 = utime.ticks_ms()
