90 lines
2.0 KiB
Python
90 lines
2.0 KiB
Python
#!/usr/bin/env python3
|
|
import os
|
|
import time
|
|
import sys
|
|
|
|
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.button import Button
|
|
|
|
import time
|
|
import logging
|
|
from datetime import datetime
|
|
|
|
now = datetime.now() # current date and time
|
|
LOGNAME = os.path.join(os.getcwd(), "logs/", (now.strftime("%m-%d_%H:%M:%S") + ".csv"))
|
|
# print(LOGNAME)
|
|
logging.basicConfig(filename=LOGNAME,
|
|
filemode='a',
|
|
format='%(message)s',
|
|
level=logging.DEBUG)
|
|
|
|
os.system('setfont Lat15-TerminusBold14')
|
|
|
|
# Motors
|
|
tank_drive = MoveTank(OUTPUT_B,OUTPUT_C)
|
|
|
|
def exit():
|
|
print('Adios')
|
|
tank_drive.stop()
|
|
sys.exit(130)
|
|
|
|
# Button
|
|
btn = Button()
|
|
btn.on_backspace = exit
|
|
|
|
# Gyro
|
|
gyro = GyroSensor(INPUT_2)
|
|
gyro.mode = GyroSensor.MODE_GYRO_RATE
|
|
|
|
print("Put system in stable position!")
|
|
for i in range(1, 0, -1):
|
|
print(i)
|
|
time.sleep(1)
|
|
|
|
gyro.calibrate()
|
|
|
|
# Keep time
|
|
dt = 0
|
|
t = time.time()
|
|
error = 0
|
|
|
|
def main():
|
|
global dt, t, error
|
|
|
|
Kp = 3
|
|
Ki = 0
|
|
Kd = 0.15
|
|
|
|
power = 0
|
|
while (True):
|
|
# Timekeepers
|
|
dt = time.time() - t
|
|
t = time.time()
|
|
|
|
# Gyro
|
|
prev_err = error
|
|
gyro_value = gyro.rate
|
|
error += (gyro_value * dt)
|
|
|
|
power = (error * Kp) + (((error - prev_err) / (dt/1000)) * Kd)
|
|
# logging.debug("%d;%d;%d", error, power)
|
|
# print("Err: ", error, "Pwr: ", power)
|
|
|
|
if (power < -100):
|
|
tank_drive.on(SpeedPercent(-100), SpeedPercent(-100))
|
|
elif (power > 100):
|
|
tank_drive.on(SpeedPercent(100), SpeedPercent(100))
|
|
else:
|
|
tank_drive.on(SpeedPercent(power), SpeedPercent(power))
|
|
|
|
# Has likely fallen over
|
|
if (error > 40 or error < -40):
|
|
exit()
|
|
|
|
if __name__ == '__main__':
|
|
try:
|
|
main()
|
|
except KeyboardInterrupt:
|
|
exit() |