-
Notifications
You must be signed in to change notification settings - Fork 27
/
crazyflie_controller_py_firmware_pid.py
189 lines (155 loc) · 5.37 KB
/
crazyflie_controller_py_firmware_pid.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
# ........... ____ _ __
# | ,-^-, | / __ )(_) /_______________ _____ ___
# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \
# | / ,..´ | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
# +....... /_____/_/\__/\___/_/ \__,_/ /___/\___/
# MIT License
# Copyright (c) 2022 Bitcraze
# @file crazyflie_controllers_py.py
# Controls the crazyflie motors in webots in Python
"""crazyflie_controller_py controller."""
from controller import Robot
from controller import Motor
from controller import InertialUnit
from controller import GPS
from controller import Gyro
from controller import Keyboard
from controller import Camera
from controller import DistanceSensor
from math import cos, sin, degrees, radians
import sys
# Change this path to your crazyflie-firmware folder
sys.path.append('../../../../../c/crazyflie-firmware')
import cffirmware
robot = Robot()
timestep = int(robot.getBasicTimeStep())
## Initialize motors
m1_motor = robot.getDevice("m1_motor")
m1_motor.setPosition(float('inf'))
m1_motor.setVelocity(-1)
m2_motor = robot.getDevice("m2_motor")
m2_motor.setPosition(float('inf'))
m2_motor.setVelocity(1)
m3_motor = robot.getDevice("m3_motor")
m3_motor.setPosition(float('inf'))
m3_motor.setVelocity(-1)
m4_motor = robot.getDevice("m4_motor")
m4_motor.setPosition(float('inf'))
m4_motor.setVelocity(1)
## Initialize Sensors
imu = robot.getDevice("inertial_unit")
imu.enable(timestep)
gps = robot.getDevice("gps")
gps.enable(timestep)
gyro = robot.getDevice("gyro")
gyro.enable(timestep)
camera = robot.getDevice("camera")
camera.enable(timestep)
range_front = robot.getDevice("range_front")
range_front.enable(timestep)
range_left = robot.getDevice("range_left")
range_left.enable(timestep)
range_back = robot.getDevice("range_back")
range_back.enable(timestep)
range_right = robot.getDevice("range_right")
range_right.enable(timestep)
## Get keyboard
keyboard = Keyboard()
keyboard.enable(timestep)
## Initialize variables
pastXGlobal = 0
pastYGlobal = 0
pastZGlobal = 0
past_time = robot.getTime()
cffirmware.controllerPidInit()
print('Take off!')
# Main loop:
while robot.step(timestep) != -1:
dt = robot.getTime() - past_time
## Get measurements
roll = imu.getRollPitchYaw()[0]
pitch = imu.getRollPitchYaw()[1]
yaw = imu.getRollPitchYaw()[2]
roll_rate = gyro.getValues()[0]
pitch_rate = gyro.getValues()[1]
yaw_rate = gyro.getValues()[2]
xGlobal = gps.getValues()[0]
vxGlobal = (xGlobal - pastXGlobal)/dt
yGlobal = gps.getValues()[1]
vyGlobal = (yGlobal - pastYGlobal)/dt
zGlobal = gps.getValues()[2]
vzGlobal = (zGlobal - pastZGlobal)/dt
## Put measurement in state estimate
# TODO replace these with a EKF python binding
state = cffirmware.state_t()
state.attitude.roll = degrees(roll)
state.attitude.pitch = -degrees(pitch)
state.attitude.yaw = degrees(yaw)
state.position.x = xGlobal
state.position.y = yGlobal
state.position.z = zGlobal
state.velocity.x = vxGlobal
state.velocity.y = vyGlobal
state.velocity.z = vzGlobal
# Put gyro in sensor data
sensors = cffirmware.sensorData_t()
sensors.gyro.x = degrees(roll_rate)
sensors.gyro.y = degrees(pitch_rate)
sensors.gyro.z = degrees(yaw_rate)
# keyboard input
forwardDesired = 0
sidewaysDesired = 0
yawDesired = 0
key = keyboard.getKey()
while key>0:
if key == Keyboard.UP:
forwardDesired = 0.5
elif key == Keyboard.DOWN:
forwardDesired = -0.5
elif key == Keyboard.RIGHT:
sidewaysDesired = -0.5
elif key == Keyboard.LEFT:
sidewaysDesired = 0.5
elif key == ord('Q'):
yawDesired = 8
elif key == ord('E'):
yawDesired = -8
key = keyboard.getKey()
## Example how to get sensor data
# range_front_value = range_front.getValue();
# cameraData = camera.getImage()
## Fill in Setpoints
setpoint = cffirmware.setpoint_t()
setpoint.mode.z = cffirmware.modeAbs
setpoint.position.z = 1.0
setpoint.mode.yaw = cffirmware.modeVelocity
setpoint.attitudeRate.yaw = degrees(yawDesired)
setpoint.mode.x = cffirmware.modeVelocity
setpoint.mode.y = cffirmware.modeVelocity
setpoint.velocity.x = forwardDesired
setpoint.velocity.y = sidewaysDesired
setpoint.velocity_body = True
## Firmware PID bindings
control = cffirmware.control_t()
tick = 100 #this value makes sure that the position controller and attitude controller are always always initiated
cffirmware.controllerPid(control, setpoint,sensors,state,tick)
##
cmd_roll = radians(control.roll)
cmd_pitch = radians(control.pitch)
cmd_yaw = -radians(control.yaw)
cmd_thrust = control.thrust
## Motor mixing
motorPower_m1 = cmd_thrust - cmd_roll + cmd_pitch + cmd_yaw
motorPower_m2 = cmd_thrust - cmd_roll - cmd_pitch - cmd_yaw
motorPower_m3 = cmd_thrust + cmd_roll - cmd_pitch + cmd_yaw
motorPower_m4 = cmd_thrust + cmd_roll + cmd_pitch - cmd_yaw
scaling = 1000 ##Todo, remove necessity of this scaling (SI units in firmware)
m1_motor.setVelocity(-motorPower_m1/scaling)
m2_motor.setVelocity(motorPower_m2/scaling)
m3_motor.setVelocity(-motorPower_m3/scaling)
m4_motor.setVelocity(motorPower_m4/scaling)
past_time = robot.getTime()
pastXGlobal = xGlobal
pastYGlobal = yGlobal
pastZGlobal = zGlobal
pass