dmnVCS3T

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