eZcRjVqu

1--------- Connect all Components ---------
2--motor FrontLeft
3motorFrontLeft = peripheral.wrap("electric_motor_12")
4--motor FrontRight
5motorFrontRight = peripheral.wrap("electric_motor_19")
6--motor BackLeft
7motorBackLeft = peripheral.wrap("electric_motor_13")
8--motor BackRight
9motorBackRight = peripheral.wrap("electric_motor_14")
10--reader
11sR = peripheral.wrap("top")
12 
13 
14---------- Initialize Variables ----------
15altitude = {p = 0, i = 0, d = 0, itime = 0, lError = 0, integral = 0}
16roll = {p, i, d, itime = 0, lError = 0, integral = 0}
17pitch = {p, i, d, itime = 0, lError = 0, integral = 0}
18 
19degProportionalConst = 90
20rawRotData = {}
21corrRotData = {["pitch"] = 0, ["roll"] = 0, ["yaw"] = 0}
22altData = 0
23nError = 0
24time = 0
25 
26 
27 
28 
29--motor speeds
30frontLeftRpm = 0
31frontRightRpm = 0
32backLeftRpm = 0
33backRightRpm = 0
34baseSpeed = 162
35 
36 
37 
38 
39--------------- PID Settings ---------------
40altitude.p = 0.01
41altitude.i = 1
42altitude.itime = 60
43altitude.d = 0
44roll.p = 0.01
45roll.i = 1
46roll.itime = 60
47roll.d = 0
48pitch.p = .77
49pitch.i = 1
50pitch.itime = 60
51pitch.d = .705
52 
53 
54desiredYaw = 0
55desiredPitch = .5
56desiredRoll = 0
57desiredAltitude = -20
58 
59 
60 
61---------------------- Functions ----------------------
62--Get Errors
63function getYawError()
64    return desiredYaw - corrRotData.yaw
65end
66 
67function getRollError()
68    return desiredRoll - corrRotData.roll
69end
70 
71function getPitchError()
72    return desiredPitch - corrRotData.pitch
73end
74 
75function getAltitudeError()
76    return desiredAltitude - altData
77end
78 
79 
80-------- read and transform data --------
81function readData() 
82    rawData = sR.getRotation()
83    corrRotData = getCorrRotData()
84    altData = sR.getWorldspacePosition().y
85end
86 
87function getCorrRotData()
88    return {
89        ["pitch"] = rawData.yaw * degProportionalConst,
90        ["roll"] = rawData.pitch * degProportionalConst,
91        ["yaw"] = rawData.roll * degProportionalConst
92    }
93end
94 
95--PID Function--
96function pid(x, err)
97    lastErr = x.lError
98    x.lError = err
99 
100    --Proportional--
101    correction = x.lError * x.p
102 
103    --Intergral
104    x.integral = (x.integral * (x.itime - 1) * x.lError)/x.itime 
105    print(x.integral)
106    correction = correction + x.integral * x.i
107 
108    --Derivative--
109    return correction + (x.lError - lastErr) * x.d 
110 
111end
112 
113function printRelevantInfo()
114    term.clear()
115    term.setCursorPos(1,1)
116    print("Drone 1.0")
117    print("Desired Altitude: "..desiredAltitude)
118    print("Desired Pitch: "..desiredPitch)
119    print("Desired Roll: "..desiredRoll)
120    print("Desired Yaw: "..desiredYaw)
121    print("Altitude Error: "..getAltitudeError())
122    print("Pitch Error: "..getPitchError())
123    print("RollError: "..getRollError())
124    print("frontLeftRpm: "..frontLeftRpm)
125    print("frontRightRpm: "..frontRightRpm)
126    print("backLeftRpm: "..backLeftRpm)
127    print("backRightRpm: "..backRightRpm)
128    print("currentPitch: "..corrRotData.pitch)
129    sleep(.05)
130end
131 
132--worker function
133function work1()
134    motorFrontLeft.setSpeed(frontLeftRpm)
135end
136
137function work2()
138    motorBackLeft.setSpeed(backLeftRpm)
139end
140
141function calcSpeeds()
142    frontLeftRpm = baseSpeed + pid(altitude,getAltitudeError()) - pid(pitch,getPitchError()) + pid(roll,getRollError()) 
143    backLeftRpm = baseSpeed + pid(altitude,getAltitudeError()) + pid(pitch,getPitchError()) + pid(roll,getRollError()) 
144end
145 
146--Init
147function init()
148    desiredAltitude = sR.getWorldspacePosition().y
149    altData = desiredAltitude
150end
151 
152--Controls
153function keyListen()
154    local event, key = os.pullEvent()
155    if key == 87 then --W
156        print("UP")
157        desiredAltitude = desiredAltitude + 5
158    elseif key == 83 then --S
159        print("DOWN")
160        desiredAltitude = desiredAltitude - 5
161    end
162end
163 
164--Ticker for parallel api
165function tick()
166    sleep(.5)
167end
168 
169--Fucntion for motors to get actuated at 3Hz
170function wait()
171    if time == 0 then
172        time = os.clock()
173    else
174        print(os.clock() - time)
175        if os.clock() - time > .34 then
176            time = 0
177            parallel.waitForAny(work1, work2)
178        end
179    end
180end
181 
182function wrap()
183    readData()
184    calcSpeeds()
185    printRelevantInfo()
186    wait()
187end
188 
189 
190
191
192--PROGRAM LOOP
193init()
194while true do
195    parallel.waitForAny(wrap,keyListen)
196    sleep(.05)
197end