126 lines
3.1 KiB
Python
126 lines
3.1 KiB
Python
#!/usr/bin/env python3
|
|
|
|
# https://github.com/ev3dev/ev3dev-lang-python
|
|
# https://github.com/ev3dev/ev3dev-lang-python-demo#balanc3r
|
|
|
|
from ev3dev2.motor import LargeMotor, OUTPUT_B, OUTPUT_C
|
|
from ev3dev2.sensor import INPUT_2
|
|
from ev3dev2.sensor.lego import GyroSensor
|
|
from ev3dev2.sound import Sound
|
|
|
|
import time
|
|
# import logging
|
|
import os
|
|
import csv
|
|
|
|
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") + "final.csv"))
|
|
# # print(LOGNAME)
|
|
# logging.basicConfig(filename=LOGNAME,
|
|
# filemode='a',
|
|
# format='%(message)s',
|
|
# level=logging.DEBUG)
|
|
|
|
log_t = []
|
|
log_ang = []
|
|
log_vang = []
|
|
log_x = []
|
|
log_v = []
|
|
|
|
# Motors
|
|
l_motor = LargeMotor(OUTPUT_B)
|
|
r_motor = LargeMotor(OUTPUT_C)
|
|
|
|
# Sensors
|
|
gyro = GyroSensor(INPUT_2)
|
|
|
|
# Sound
|
|
sound = Sound()
|
|
|
|
# tires are 56mm diameter
|
|
def balance(target_angle=0):
|
|
t = time.time()
|
|
|
|
# PID
|
|
gyro_k_p = 7.5
|
|
# float k_i = 0 # No integral in this system
|
|
gyro_k_d = 1.15
|
|
motor_k_p = 0.07
|
|
motor_k_d = 0.1
|
|
|
|
# Calibrate gyro in current position
|
|
gyro.calibrate()
|
|
gyro.mode = GyroSensor.MODE_GYRO_G_A
|
|
angle = gyro.angle
|
|
|
|
# Calibrate motor
|
|
prev_sum_motor_pos = 0
|
|
l_motor.reset()
|
|
r_motor.reset()
|
|
pos = 0
|
|
|
|
sound.speak('3, 2, 1')
|
|
|
|
# Stop if the robot has fallen over
|
|
while abs(angle) < 40:
|
|
# Keep time
|
|
dt = time.time() - t
|
|
t = time.time()
|
|
|
|
sum_motor_pos = l_motor.position + r_motor.position
|
|
|
|
delta_motor_pos = sum_motor_pos - prev_sum_motor_pos
|
|
pos += delta_motor_pos
|
|
speed = (delta_motor_pos / dt)
|
|
|
|
prev_sum_motor_pos = sum_motor_pos
|
|
|
|
angle, rate = gyro.angle_and_rate
|
|
|
|
log_t.append(t)
|
|
log_ang.append(angle)
|
|
log_vang.append(rate)
|
|
log_x.append(pos)
|
|
log_v.append(speed)
|
|
|
|
error = angle - target_angle
|
|
pd = gyro_k_p * error + gyro_k_d * rate \
|
|
+ motor_k_p * pos + motor_k_d * speed
|
|
|
|
# convert -100 ~ 100 to -1050 ~ 1050
|
|
speed = (((pd - (-100)) * (1049 - (-1049))) / (100 - (-100))) + (-1049)
|
|
l_motor.run_forever(speed_sp=speed)
|
|
r_motor.run_forever(speed_sp=speed)
|
|
|
|
# logging.debug("%d;%d;%d", angle, rate, pd)
|
|
|
|
# if abs(pd) > 1050:
|
|
# print("cap")
|
|
|
|
# speed = min(max(pd, -1050), 1050)
|
|
|
|
l_motor.stop(stop_action="hold")
|
|
r_motor.stop(stop_action="hold")
|
|
|
|
|
|
# Combine the arrays
|
|
try:
|
|
balance()
|
|
except:
|
|
l_motor.stop(stop_action="hold")
|
|
r_motor.stop(stop_action="hold")
|
|
finally:
|
|
data = zip(log_t, log_ang, log_vang, log_x, log_v)
|
|
|
|
# Define the filename
|
|
now = datetime.now() # current date and time
|
|
LOGNAME = os.path.join(now.strftime("%m-%d_%H:%M:%S") + "new.csv")
|
|
|
|
# Write data to CSV file
|
|
with open(LOGNAME, 'w', newline='') as csvfile:
|
|
csvwriter = csv.writer(csvfile)
|
|
csvwriter.writerow(['Time', 'Angle', 'Angular Velocity', 'X pos', 'Speed']) # Write header row
|
|
csvwriter.writerows(data)
|