44 lines
908 B
Python
44 lines
908 B
Python
#!/usr/bin/env python3
|
|
import os
|
|
import time
|
|
|
|
from ev3dev2.motor import LargeMotor, OUTPUT_B, OUTPUT_C, SpeedPercent, MoveTank
|
|
from ev3dev2.sensor import INPUT_2
|
|
from ev3dev2.sensor.lego import GyroSensor
|
|
from ev3dev2.led import Leds
|
|
|
|
os.system('setfont Lat15-TerminusBold14')
|
|
|
|
# tank_drive = MoveTank(OUTPUT_B,OUTPUT_C)
|
|
|
|
gyro = GyroSensor(INPUT_2)
|
|
gyro.mode = GyroSensor.MODE_GYRO_RATE
|
|
|
|
dt = 0
|
|
t = time.time()
|
|
|
|
error = 0.0
|
|
total = 0
|
|
for i in range(10):
|
|
total += gyro.rate
|
|
time.sleep(0.01)
|
|
average = total / 10.0
|
|
offset = average - 0.1
|
|
|
|
GYRO_OFFSET = 0.0005
|
|
|
|
while (True):
|
|
# Timekeepers
|
|
dt = time.time() - t
|
|
t = time.time()
|
|
|
|
gyro_raw = gyro.rate
|
|
offset = GYRO_OFFSET * gyro_raw + (1 - GYRO_OFFSET) * offset
|
|
speed = (gyro_raw - offset)
|
|
error += speed * (dt/1000)
|
|
|
|
# # Gyro
|
|
# gyro_value = gyro.rate
|
|
# error += (gyro_value * dt)
|
|
|
|
print("Gyro: " + str(error)) |