class Controller(Flask):
SUCCESS_MSG_ACCEPTED = "Success", status.HTTP_202_ACCEPTED
ERR_MSG_MOTOR_NAME = "Invalid motor name", status.HTTP_406_NOT_ACCEPTABLE
ERR_MSG_BAD_FORMAT = "Invalid json format", status.HTTP_406_NOT_ACCEPTABLE
def __init__(self):
Flask.__init__(self, "controller")
self.__motors = {}
self.__motors["red_cw_1"] = Motor(5, "red_cw_1")
self.__motors["black_ccw_1"] = Motor(6, "black_ccw_1")
self.__motors["red_cw_2"] = Motor(13, "red_cw_2")
self.__motors["black_ccw_2"] = Motor(19, "black_ccw_2")
self.__mpu = MPU()
# Define control routes
self.add_url_rule("/", view_func=self.view_root, methods=["GET"])
self.add_url_rule("/accel", view_func=self.view_accel, methods=["GET"])
self.add_url_rule("/gyro", view_func=self.view_gyro, methods=["GET"])
self.add_url_rule("/motor/<string:name>",
view_func=self.view_motor,
methods=["GET"])
self.add_url_rule("/speed/<string:name>",
view_func=self.set_speed,
methods=["GET", "POST"])
def view_root(self):
return Controller.SUCCESS_MSG_ACCEPTED
def view_gyro(self):
return json.dumps(self.__mpu.gyro())
def view_accel(self):
return json.dumps(self.__mpu.accel())
def view_motor(self, name):
try:
return str(self.__motors[name])
except:
return Controller.ERR_MSG_MOTOR_NAME
def set_speed(self, name):
try:
motor = self.__motors[name]
except:
return Controller.ERR_MSG_MOTOR_NAME
try:
post = json.loads(request.form.keys()[0])
print(post)
if "percent" in post:
motor.set_percentage(float(post["percent"]))
return Controller.SUCCESS_MSG_ACCEPTED
if "pulse_width" in post:
motor.set_pulse_width(int(post["pulse_width"]))
return Controller.SUCCESS_MSG_ACCEPTED
return Controller.ERR_MSG_BAD_FORMAT
except:
return Controller.ERR_MSG_BAD_FORMAT
def __init__(self):
Flask.__init__(self, "controller")
self.__motors = {}
self.__motors["red_cw_1"] = Motor(5, "red_cw_1")
self.__motors["black_ccw_1"] = Motor(6, "black_ccw_1")
self.__motors["red_cw_2"] = Motor(13, "red_cw_2")
self.__motors["black_ccw_2"] = Motor(19, "black_ccw_2")
self.__mpu = MPU()
# Define control routes
self.add_url_rule("/", view_func=self.view_root, methods=["GET"])
self.add_url_rule("/accel", view_func=self.view_accel, methods=["GET"])
self.add_url_rule("/gyro", view_func=self.view_gyro, methods=["GET"])
self.add_url_rule("/motor/<string:name>",
view_func=self.view_motor,
methods=["GET"])
self.add_url_rule("/speed/<string:name>",
view_func=self.set_speed,
methods=["GET", "POST"])
def __init__(self):
self.previousPowerLevel = 0
self.powerLevel = 0
self.xAngleDeflection = 5
self.yAngleDeflection = 5
self.mpu = MPU()
self.mpu.start()
self.pidSampletime = 0.01
self.xPID = PID(0.01 * 72, 0.01 * 19, 0.01 * 9, 0)
self.xPID.sample_time = self.pidSampletime
self.xPID.output_limits = (-20, 20)
self.yPID = PID(0.01 * 81, 0.01 * 13, 0.01 * 14, 0)
self.yPID.sample_time = self.pidSampletime
self.yPID.output_limits = (-20, 20)
self.xPID.auto_mode = False
self.yPID.auto_mode = False
self.ended = False
self.s = socket.socket()
self.s.bind(("", 5005))
self.s.listen()
self.i2c = busio.I2C(board.SCL, board.SDA)
self.pca = adafruit_pca9685.PCA9685(self.i2c)
self.pca.frequency = 2000
self.kit = ServoKit(channels=8)
self.m1 = self.kit.servo[0]
self.m2 = self.kit.servo[1]
self.m3 = self.kit.servo[2]
self.m4 = self.kit.servo[3]
self.m1Val = 0
self.m2Val = 0
self.m3Val = 0
self.m4Val = 0
self.arm()
threading.Thread(target=self.pidCorrectionThread).start()
self.activeClient = False
self.awaitClients()
#Author : Roche Christopher
#File created on 21 Aug 2019 9:48 PM
from mpu import MPU
import math
import time
# variables
rad2deg = 57.2957786
# device address
device_address = 0X68
mpu6050 = MPU(device_address)
mpu6050.initialize(gyro_config=int('00001000', 2),
smplrt_div_value=1,
general_config=int('00000110', 2),
accelerometer_config=int('00011000', 2))
#gyro related variables
gyro_to_angle_dt = 65.5
accl_config_const = 2048
dt = 0.05 # 10 ms -- Changing the sampling time will affect the output values. DO NOT DO IT!!!!!
print("calibrating gyroscope and accelerometer")
gyro_x_offset = 0
gyro_y_offset = 0
gyro_z_offset = 0
accl_x_offset = 0
accl_y_offset = 0
def __init__(self): #Construtor da Classe Carro: instancia os objetos de interesse
self._estercamento = Estercamento()
self._freio = Freio()
self._motor = Motor()
self._ultrassonico = Ultrassonico()
self._mpu = MPU()
# gyro_z_out_l = 0X48
#
# # accelerometer register addresses
# accl_x_out_h = 0X3B
# accl_x_out_l = 0X3C
# accl_y_out_h = 0X3D
# accl_y_out_l = 0X3E
# accl_z_out_h = 0X3F
# accl_z_out_l = 0X40
#
# # temperature sensor register address
#
# temp_out_h = 0X41
# temp_out_h = 0X42
mpu6050 = MPU(device_address)
mpu6050.initialize()
while True:
gyro_x = mpu6050.get_gyro_x()
gyro_y = mpu6050.get_gyro_y()
gyro_z = mpu6050.get_gyro_z()
accl_x = mpu6050.get_accl_x()
accl_y = mpu6050.get_accl_y()
accl_z = mpu6050.get_accl_z()
temperature = mpu6050.get_temp_data()
print(gyro_x, gyro_y, gyro_z, " | ", accl_x, accl_y, accl_z, " | ", temperature)
time.sleep(0.2)
class Server:
def __init__(self):
self.previousPowerLevel = 0
self.powerLevel = 0
self.xAngleDeflection = 5
self.yAngleDeflection = 5
self.mpu = MPU()
self.mpu.start()
self.pidSampletime = 0.01
self.xPID = PID(0.01 * 72, 0.01 * 19, 0.01 * 9, 0)
self.xPID.sample_time = self.pidSampletime
self.xPID.output_limits = (-20, 20)
self.yPID = PID(0.01 * 81, 0.01 * 13, 0.01 * 14, 0)
self.yPID.sample_time = self.pidSampletime
self.yPID.output_limits = (-20, 20)
self.xPID.auto_mode = False
self.yPID.auto_mode = False
self.ended = False
self.s = socket.socket()
self.s.bind(("", 5005))
self.s.listen()
self.i2c = busio.I2C(board.SCL, board.SDA)
self.pca = adafruit_pca9685.PCA9685(self.i2c)
self.pca.frequency = 2000
self.kit = ServoKit(channels=8)
self.m1 = self.kit.servo[0]
self.m2 = self.kit.servo[1]
self.m3 = self.kit.servo[2]
self.m4 = self.kit.servo[3]
self.m1Val = 0
self.m2Val = 0
self.m3Val = 0
self.m4Val = 0
self.arm()
threading.Thread(target=self.pidCorrectionThread).start()
self.activeClient = False
self.awaitClients()
def measureVoltageThread(self, con):
self.ads = ADS.ADS1115(self.i2c)
while not self.ended and self.connected:
time.sleep(5)
self.voltageChan = AnalogIn(self.ads, ADS.P1)
voltage = self.voltageChan.voltage * (683 + 220) / 220
value = int((voltage - 4 * 3.7) * 50)
try:
con.send(("Voltage: " + str(value)).encode())
except:
pass
def awaitClients(self):
while not self.ended:
try:
(con, adr) = self.s.accept()
(videoCon, _) = self.s.accept()
print("Connection by " + str(adr))
threading.Thread(target=self.authorizeClient,
args=[adr, con, videoCon]).start()
except:
pass
def authorizeClient(self, adr, con, videoCon):
#con.setblocking(0)
ready = select.select([con], [], [], 4)
if ready[0]:
data = con.recv(1024).decode()
if data == "hello" and not self.activeClient:
self.adr = adr
self.con = con
self.activeClient = True
self.connected = True
threading.Thread(target=self.measureVoltageThread,
args=[con]).start()
videoResponse = select.select([videoCon], [], [], 4)
if videoResponse[0]:
data = videoCon.recv(1024).decode()
if data == "video":
threading.Thread(target=self.sendImages,
args=[videoCon]).start()
self.handleClient()
con.close()
print("Connection closed " + str(adr) + "\n")
def sendImages(self, videoCon):
if self.connected:
with PiCamera() as camera:
camera.resolution = (640, 480)
camera.framerate = 30
rawCapture = PiRGBArray(camera, size=(640, 480))
for frame in camera.capture_continuous(rawCapture,
format="bgr",
use_video_port=True):
if not self.connected:
break
img = frame.array
img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
img = cv2.flip(img, 0)
data = pickle.dumps(img)
size = len(data)
videoCon.sendall(struct.pack("Q", size) + data)
rawCapture.truncate(0)
time.sleep(1 / 30)
def end(self):
print("ending Server")
self.connected = False
try:
self.xPID.auto_mode = False
self.yPID.auto_mode = False
self.con.send("close: Server trennt Verbindung".encode())
except:
pass
self.con.close()
self.activeClient = False
self.ended = True
sys.exit()
def handleClient(self):
try:
while (not self.ended):
ready = select.select([self.con], [], [], 4)
if ready[0]:
mes = self.con.recv(1024).decode()
mes = mes.split(" . ")[-2]
if (mes):
#print(mes)
mesSplit = mes.split(" ")
if (mes == "close"):
raise
elif (mes == "kill"):
self.powerLevel = 0
self.stopMotors()
print("Stop everything")
elif (mes == "resetGyro"):
self.mpu.resetGyro()
elif (mes == "end"):
self.end()
elif (mesSplit[0] == "setAllCalc"):
self.setAllMotorsCalc(int(mesSplit[1]))
print("Calc: Setze alle Motoren auf " +
mesSplit[1])
elif (mesSplit[0] == "setAllRaw"):
self.setAllMotorsRaw(int(mesSplit[1]))
print("Raw: Setze alle Motoren auf " + mesSplit[1])
elif (mesSplit[0] == "setMotorCalc"):
if (mesSplit[1] == "M1" or mesSplit[1] == "M2"
or mesSplit[1] == "M3"
or mesSplit[1] == "M4"):
self.setMotorRaw(mesSplit[1],
int(mesSplit[2]) + 80)
print("Motor " + mesSplit[1] + " DCalc:" +
mesSplit[2])
elif (mesSplit[0] == "setMotorRaw"):
if (mesSplit[1] == "M1" or mesSplit[1] == "M2"
or mesSplit[1] == "M3"
or mesSplit[1] == "M4"):
self.setMotorRaw(mesSplit[1], int(mesSplit[2]))
print("Motor " + mesSplit[1] + " DRaw:" +
mesSplit[2])
elif (mesSplit[0] == "pid"):
if mesSplit[1] == "X":
pid = self.xPID
pidCommand = mes.split("X")[1][1:]
else:
pid = self.yPID
pidCommand = mes.split("Y")[1][1:]
print(pidCommand)
if pidCommand.startswith("p:"):
print(pidCommand.split("p:")[1])
pid.Kp = float(pidCommand.split("p:")[1])
elif pidCommand.startswith("i:"):
print(pidCommand.split("i:")[1])
pid.Ki = float(pidCommand.split("i:")[1])
elif pidCommand.startswith("d:"):
print(pidCommand.split("d:")[1])
pid.Kd = float(pidCommand.split("d:")[1])
print("set pid Axis:" + mesSplit[1] + " K:" +
str(pid.Kp) + " I:" + str(pid.Ki) + " D:" +
str(pid.Kd))
elif (mesSplit[0] == "MPUSlider"):
print(mesSplit[1])
self.mpu.setMPUComVal(float(mesSplit[1]))
elif (mesSplit[0] == "keyCom"):
print(mes)
if (mes.split("p:")[1]):
self.powerLevel = int(mes.split("p:")[1]) / 2
x = mes.split("X:")[1].split(" ")[0]
if (x == "f"):
self.xPID.setpoint = self.xAngleDeflection
print(self.xPID.components)
elif (x == "b"):
self.xPID.setpoint = -1 * self.xAngleDeflection
else:
self.xPID.setpoint = 0
y = mes.split("Y:")[1].split(" ")[0]
if (y == "l"):
self.yPID.setpoint = self.yAngleDeflection
elif (y == "r"):
self.yPID.setpoint = -1 * self.yAngleDeflection
else:
self.yPID.setpoint = 0
except Exception as e:
print("error " + str(e))
self.setAllMotorsCalc(0)
self.connected = False
try:
self.con.send("close: Server trennt Verbindung".encode())
except:
pass
self.con.close()
self.activeClient = False
print("Connection closed " + str(self.adr))
def pidCorrectionThread(self):
global plot
print(plot)
if plot:
plotListX = []
plotListY = []
cycles = []
i = 0
while not self.ended:
self.xAngle = self.mpu.sumGyroX
self.yAngle = self.mpu.sumGyroY
self.zAngle = self.mpu.sumGyroZ
try:
self.con.send(
("Gyro: " + str(self.xAngle) + " " + str(self.yAngle) +
" " + str(self.zAngle)).encode())
except:
pass
i += 1
if (self.powerLevel > 5):
self.xPID.auto_mode = True
self.yPID.auto_mode = True
xPIDOutput = self.xPID(self.xAngle)
#xPIDOutput=0
yPIDOutput = self.yPID(self.yAngle)
#yPIDOutput = 0
#print(xPIDOutput)
else:
self.xPID.auto_mode = False
self.yPID.auto_mode = False
xPIDOutput = 0
yPIDOutput = 0
cycles += [i]
if self.xPID.auto_mode:
self.setMotorCalc("M1",
self.powerLevel - xPIDOutput - yPIDOutput)
self.setMotorCalc("M2",
self.powerLevel - xPIDOutput + yPIDOutput)
self.setMotorCalc("M3",
self.powerLevel + xPIDOutput + yPIDOutput)
self.setMotorCalc("M4",
self.powerLevel + xPIDOutput - yPIDOutput)
else:
self.setAllMotorsCalc(self.powerLevel)
if plot:
plotListX += [self.xAngle]
plotListY += [self.yAngle]
#print(xPIDOutput,yPIDOutput)
time.sleep(.01)
self.stopMotors()
if (plot):
plt.plot(cycles, plotListX, label="X-Values")
plt.plot(cycles, plotListY, label="Y-Values")
plt.xlabel("Cycles")
plt.ylabel("Degrees")
plt.title("Imu during flight")
plt.legend()
plt.savefig("Imu.png")
#print(plotListX)
def arm(self):
self.setAllMotorsRaw(80)
time.sleep(2)
def stopMotors(self):
self.setAllMotorsRaw(80)
def setAllMotorsCalc(self, n):
self.setAllMotorsRaw(80 + n)
def setAllMotorsRaw(self, n):
self.m1.angle = n
self.m2.angle = n
self.m3.angle = n
self.m4.angle = n
def setMotorRaw(self, m, n):
if (m == "M1"):
self.m1.angle = n
elif (m == "M2"):
self.m2.angle = n
elif (m == "M3"):
self.m3.angle = n
elif (m == "M4"):
self.m4.angle = n
def setMotorCalc(self, m, n):
self.setMotorRaw(m, 80 + n)
#Author : Roche Christopher
#File created on 21 Aug 2019 8:53 PM
# The following code is based on the assumption that no linear force is exerted on the sensor
# Assumtion2: Orientation of the craft on which the sensor is mounted is in such a way that the x - axis is pitch axis. i.e pitch happens with respect to x axis
from mpu import MPU
import math
import time
# variables
rad2deg = 57.3
# device address
device_address = 0X68
mpu6050 = MPU(device_address)
mpu6050.initialize(accelerometer_config=int('00011000', 2), smplrt_div_value=0)
accl_config_const = 2048 #2048 LSB
dt = 0.1 # 1 deka sec i.e 0.1 sec
#Restrict pitch from -90 to +90 -- enables roll to be measured from 180 to -180
restrict_pitch = True
accl_x = 0
accl_y = 0
accl_z = 0
pitch = 0
roll = 0
while True:
#Author : Roche Christopher
#File created on 18 Aug 2019 10:04 AM
from mpu import MPU
import time
# variables
# device address
device_address = 0X68
mpu6050 = MPU(device_address)
#gyro related variables
mpu6050.initialize(gyro_config=int('00001000', 2),
smplrt_div_value=0,
general_config=6)
gyro_to_angle_dt = 65.5 # 4000 / 2**16
dt = 0.006 # 1 ms
print("calibrating gyroscope")
gyro_x_offset = 0
gyro_y_offset = 0
gyro_z_offset = 0
for i in range(2000):
gyro_x_offset += mpu6050.get_gyro_x()
gyro_y_offset += mpu6050.get_gyro_y()
gyro_z_offset += mpu6050.get_gyro_z()
time.sleep(0.001)
def measure_angles(self):
# variables
rad2deg = 57.2957786
# device address
device_address = 0X68
mpu6050 = MPU(device_address)
mpu6050.initialize(gyro_config=int('00001000', 2),
smplrt_div_value=1,
general_config=int('00000110', 2),
accelerometer_config=int('00011000', 2))
# gyro related variables
gyro_to_angle_dt = 65.5
accl_config_const = 2048
dt = 0.05 # 10 ms -- Changing the sampling time will affect the output values. DO NOT DO IT!!!!!
print("calibrating gyroscope and accelerometer")
gyro_x_offset = 0
gyro_y_offset = 0
gyro_z_offset = 0
accl_x_offset = 0
accl_y_offset = 0
accl_z_offset = 0
samples = 100
for i in range(samples):
gyro_x_offset += mpu6050.get_gyro_x()
gyro_y_offset += mpu6050.get_gyro_y()
gyro_z_offset += mpu6050.get_gyro_z()
time.sleep(0.001)
gyro_x_offset /= samples
gyro_y_offset /= samples
gyro_z_offset /= samples
accl_x = mpu6050.get_accl_x()
accl_y = mpu6050.get_accl_y()
accl_z = mpu6050.get_accl_z()
accl_angle_y_offset = round(math.atan2(accl_x, accl_z) * rad2deg,
2) # calculated pitch
accl_angle_x_offset = round(
math.atan(-accl_y / math.sqrt((accl_x**2) + (accl_z**2))) *
rad2deg, 2) # Calculated roll
print('gyroscope offsets x, y, z ', gyro_x_offset, gyro_y_offset,
gyro_z_offset)
prev_gyro_angle_x = 0
prev_gyro_angle_y = 0
gyro_angle_x = 0
gyro_angle_y = 0
gyro_angle_x_change = 0
gyro_angle_y_change = 0
prev_accl_angle_x = 0
prev_accl_angle_y = 0
accl_angle_x = 0
accl_angle_y = 0
accl_angle_x_change = 0
accl_angle_y_change = 0
trust_accl_angle_x = trust_accl_angle_y = False
angle_x = 0
angle_y = 0
accl_trust_factor = 2
prev_time = time.time()
while True:
# Angle calculation from gyroscope
gyro_x = mpu6050.get_gyro_x() - gyro_x_offset
gyro_y = mpu6050.get_gyro_y() - gyro_y_offset
gyro_z = mpu6050.get_gyro_z() - gyro_z_offset
gyro_angle_x_dt = int(gyro_x / gyro_to_angle_dt)
gyro_angle_y_dt = int(gyro_y / gyro_to_angle_dt)
gyro_angle_z_dt = int(gyro_z / gyro_to_angle_dt)
prev_gyro_angle_x = gyro_angle_x
prev_gyro_angle_y = gyro_angle_y
gyro_angle_x = round(gyro_angle_x + (gyro_angle_x_dt * dt), 2)
gyro_angle_y = round(gyro_angle_y + (gyro_angle_y_dt * dt), 2)
# print(gyro_angle_x, gyro_angle_y)
# Angle calculation from accelerometer
accl_x = mpu6050.get_accl_x()
accl_y = mpu6050.get_accl_y()
accl_z = mpu6050.get_accl_z()
prev_accl_angle_x = accl_angle_x
prev_accl_angle_y = accl_angle_y
accl_angle_y = round(math.atan2(accl_x, accl_z) * rad2deg,
2) # calculated pitch
accl_angle_x = round(
math.atan(-accl_y / math.sqrt((accl_x**2) + (accl_z**2))) *
rad2deg, 2) # Calculated roll
accl_angle_x -= accl_angle_x_offset
accl_angle_y -= accl_angle_y_offset
# print(gyro_angle_x, gyro_angle_y)
# print(gyro_angle_x, accl_angle_x, gyro_angle_y, accl_angle_y)
# Calculate change in angles
gyro_angle_x_change = abs(prev_gyro_angle_x - gyro_angle_x)
gyro_angle_y_change = abs(prev_gyro_angle_y - gyro_angle_y)
accl_angle_x_change = abs(prev_accl_angle_x - accl_angle_x)
accl_angle_y_change = abs(prev_accl_angle_y - accl_angle_y)
trust_accl_angle_x = trust_accl_angle_y = False
angle_x = gyro_angle_x
angle_y = gyro_angle_y
if int(gyro_angle_x_change):
if abs(gyro_angle_x_change -
accl_angle_x_change) < accl_trust_factor:
if abs(gyro_angle_x - accl_angle_x) < accl_trust_factor:
# print("X uses accl -- motion")
trust_accl_angle_x = True
# angle_x = (0.6 * gyro_angle_x) +(0.4 * accl_angle_x)
# gyro_angle_x = angle_x
else:
# print("X -- accl moving fast")
pass
else:
# No change in gyro values
if not int(accl_angle_x_change):
if abs(gyro_angle_x - accl_angle_x) < 10:
# print("X uses accl -- stable")
trust_accl_angle_x = True
else:
# print("X -- accl alone moving")
pass
if int(gyro_angle_y_change):
if abs(gyro_angle_y_change -
accl_angle_y_change) < accl_trust_factor:
if abs(gyro_angle_y - accl_angle_y) < accl_trust_factor:
# print("Y uses accl -- motion")
trust_accl_angle_y = True
# angle_y = (0.6 * gyro_angle_y) +(0.4 * accl_angle_y)
# gyro_angle_y = angle_y
else:
# print("y -- accl moving fast")
pass
else:
# No change in gyro values
if not int(accl_angle_y_change):
if abs(gyro_angle_y - accl_angle_y) < 10:
# print("Y uses accl -- stable")
trust_accl_angle_y = True
else:
# print("Y -- accl alone moving")
pass
if (trust_accl_angle_x and trust_accl_angle_y):
angle_x = (0.6 * gyro_angle_x) + (0.4 * accl_angle_x)
gyro_angle_x = angle_x
angle_y = (0.6 * gyro_angle_y) + (0.4 * accl_angle_y)
gyro_angle_y = angle_y
'''if(trust_accl_angle_y):
angle_y = (0.6 * gyro_angle_y) +(0.4 * accl_angle_y)
gyro_angle_y = angle_y '''
angle_x = round(angle_x, 2)
angle_y = round(angle_y, 2)
#print(angle_x, angle_y)
self.set_angle_x(angle_x)
self.set_angle_y(angle_y)
while (time.time() - prev_time) < dt:
# print("untul")
time.sleep(0.001)
pass
# print(time.time() - prev_time)
prev_time = time.time()