Project

General

Profile

Statistics
| Branch: | Revision:

root / quad1 / Documentation / Architecture / pseudoCode / pseudoCode.pde @ 9240aaa3

History | View | Annotate | Download (3.41 KB)

1
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
}