root / quad1 / Documentation / Architecture / pseudoCode / pseudoCode.pde @ 9240aaa3
History | View | Annotate | Download (3.41 KB)
1 | 9240aaa3 | Alex | void setup() { |
---|---|---|---|
2 | flightCommmand.init(type, frequency); //type = PWM, serial |
||
3 | motorCommand.init(config, type, frequency); //config = +,X,octo type = PWM,I2C |
||
4 | attitudeSensor.init(frequency); |
||
5 | cameraStabilization.init(frequency); |
||
6 | GPS.init(frequency); |
||
7 | videoDisplay.init(frequency); |
||
8 | lightPattern.init(frequency); |
||
9 | batteryMonitor.init(frequency); |
||
10 | remoteCommand.init(frequency); |
||
11 | telemetry.init(frequency); |
||
12 | |||
13 | filter.init(param); |
||
14 | } |
||
15 | |||
16 | void loop() { |
||
17 | if (flightCommand.process()) { |
||
18 | rawCommand.throttle = flightCommand.read(THROTTLE); |
||
19 | rawCommand.roll = flightCommand.read(ROLL); |
||
20 | rawCommand.pitch = flightCommand.read(PITCH); |
||
21 | rawCommand.yaw = flightCommand.read(YAW); |
||
22 | command = filter(rawCommand); |
||
23 | switch(flightCommand.check()) { |
||
24 | case CALIBRATE: |
||
25 | angleSensor.calibrate(); |
||
26 | rateSensor.calibrate(); |
||
27 | altitudeSensor.calibrate(); |
||
28 | break; |
||
29 | case ARM: |
||
30 | motor.arm(); |
||
31 | break; |
||
32 | case DISARM: |
||
33 | motor.disarm(); |
||
34 | break; |
||
35 | default: |
||
36 | break; |
||
37 | } |
||
38 | } |
||
39 | |||
40 | if (GPS.process()) { |
||
41 | attitude.longitude = GPS.read(LONGITUDE); |
||
42 | attitude.latitude = GPS.read(LATITUDE); |
||
43 | attitude.altitudeGPS = GPS.read(ALTITUDE); |
||
44 | } |
||
45 | |||
46 | if (attitudeSensor.process()) { |
||
47 | rawAttitude.rate = attitudeSensor.read(RATE); |
||
48 | rawAattitude.angle = attitudeSensor.read(ANGLE); |
||
49 | rawAattitude.altitudeSensor = attitudeSensor.read(ALTITUDE); |
||
50 | rawAattitude.heading = attitudeSensor.read(HEADING); |
||
51 | |||
52 | attitude = filter(rawAttitude); |
||
53 | direction = flightControl.calculateAttitude(command, attitude); |
||
54 | } |
||
55 | |||
56 | if (motorCommand.process()) { |
||
57 | motorCommand.throttle(direction); |
||
58 | switch(motorCommand.type()) { |
||
59 | case: PLUS |
||
60 | motorCommand.write(FRONT, direction); |
||
61 | motorCommand.write(REAR, direction); |
||
62 | motorCommand.write(LEFT, direction); |
||
63 | motorCommand.write(RIGHT, direction); |
||
64 | break; |
||
65 | case: X |
||
66 | motorCommand.write(FRONTLEFT, direction); |
||
67 | motorCommand.write(FRONTRIGHT, direction); |
||
68 | motorCommand.write(REARLEFT, direction); |
||
69 | motorCommand.write(REARRIGHT, direction); |
||
70 | break; |
||
71 | case: OCTO |
||
72 | motorCommand.write(FRONT, direction); |
||
73 | motorCommand.write(REAR, direction); |
||
74 | motorCommand.write(LEFT, direction); |
||
75 | motorCommand.write(RIGHT, direction); |
||
76 | motorCommand.write(FRONTLEFT, direction); |
||
77 | motorCommand.write(FRONTRIGHT, direction); |
||
78 | motorCommand.write(REARLEFT, direction); |
||
79 | motorCommand.write(REARRIGHT, direction); |
||
80 | break; |
||
81 | case: HEXA |
||
82 | motorCommand.write(FRONTLEFT, direction); |
||
83 | motorCommand.write(LEFT, direction); |
||
84 | motorCommand.write(REARLEFT, direction); |
||
85 | motorCommand.write(FRONTRIGHT, direction); |
||
86 | motorCommand.write(RIGHT, direction); |
||
87 | motorCommand.write(REARRIGHT, direction); |
||
88 | default: |
||
89 | break; |
||
90 | } |
||
91 | } |
||
92 | |||
93 | if (cameraStabilization.process()) { |
||
94 | cameraStabilization.write(ROLL, attitude.roll); |
||
95 | cameraStabilization.write(PITCH, attitude.pitch); |
||
96 | } |
||
97 | |||
98 | if (batteryMonitor.process()) { |
||
99 | batteryState = batteryMonitor.read(); |
||
100 | batteryMonitor.alarm(batteryState); |
||
101 | } |
||
102 | |||
103 | if (videoDisplay.process()) { |
||
104 | videoDisplay.write(attitude, batteryState); |
||
105 | } |
||
106 | |||
107 | if (lightPattern.process()) { |
||
108 | lightPattern.write(); |
||
109 | } |
||
110 | |||
111 | if (remoteCommand.process()) { |
||
112 | cmd = remoteCommand.read(); |
||
113 | remoteCommand.execute(cmd); |
||
114 | } |
||
115 | |||
116 | if (telemetry.process()) { |
||
117 | telemetry.write(); |
||
118 | } |
||
119 | } |