Advanced Rocket Flight Computer

1// 2020 Delta Space Systems //
2 float PIDX, PIDY, errorX, errorY, previous_errorX,   previous_errorY, pwmX, pwmY;
3#include <Wire.h>
4#include <Kalman.h> // Source:   https://github.com/TKJElectronics/KalmanFilter
5#include <Servo.h>
6#define   RESTRICT_PITCH // Comment out to restrict roll to 90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
7Servo   servoY;
8Servo servoX;
9Kalman kalmanX; // Create the Kalman instances
10Kalman   kalmanY;
11
12double accX, accY, accZ;
13double gyroX, gyroY, gyroZ;
14int desired_angleX   = 193;//servoY
15int desired_angleY = -18; // servoX
16double gyroXangle, gyroYangle;   // Angle calculate using the gyro only
17double kalAngleX, kalAngleY; // Calculated   angle using a Kalman filter
18int ledblu=2;
19int ledgrn=5;
20int ledred=6;
21int   buzzer=21;
22uint32_t timer;
23uint8_t i2cData[14]; // Buffer for I2C data
24float   pidX_p=0;
25float pidX_i=0;
26float pidX_d=0;
27float pidY_p=0;
28float pidY_d=0;
29/////////////////PID   CONSTANTS/////////////////
30double kp=0.16;//3.55
31double ki=0;//2.05
32double   kd=0.045;//2.05
33///////////////////////////////////////////////
34int State   = 0;
35void setup() {
36  pinMode(ledblu, OUTPUT);
37  pinMode(ledgrn, OUTPUT);
38   pinMode(ledred, OUTPUT);
39  pinMode(buzzer, OUTPUT);
40 
41  servoY.attach(30);
42    servoX.attach(29);
43  Serial.begin(115200);
44  Wire.begin();
45#if ARDUINO   >= 157
46  Wire.setClock(400000UL); // Set I2C frequency to 400kHz
47#else
48   TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
49#endif
50
51   i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
52  i2cData[1]   = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8   KHz sampling
53  i2cData[2] = 0x00; // Set Gyro Full Scale Range to 250deg/s
54   i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to 2g
55  while (i2cWrite(0x19,   i2cData, 4, false)); // Write to all four registers at once
56  while (i2cWrite(0x6B,   0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
57
58   while (i2cRead(0x75, i2cData, 1));
59  if (i2cData[0] != 0x68) { // Read "WHO_AM_I"   register
60    Serial.print(F("Error reading sensor"));
61    while (1);
62   }
63  delay(100); // Wait for sensor to stabilize
64  while (i2cRead(0x3B, i2cData,   6));
65  accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
66  accY = (int16_t)((i2cData[2]   << 8) | i2cData[3]);
67  accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
68
69#ifdef   RESTRICT_PITCH // Eq. 25 and 26
70  double roll  = atan2(accY, accZ) * RAD_TO_DEG;
71   double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
72#else   // Eq. 28 and 29
73  double roll  = atan(accY / sqrt(accX * accX + accZ * accZ))   * RAD_TO_DEG;
74  double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
75#endif
76   kalmanX.setAngle(roll); // Set starting angle
77  kalmanY.setAngle(pitch);
78   gyroXangle = roll;
79  gyroYangle = pitch;
80 
81  timer = micros();
82  digitalWrite(ledgrn,   HIGH);
83  servoX.write(93);
84  servoY.write(103);
85  delay(1000);
86  digitalWrite(ledgrn,   LOW);
87  digitalWrite(buzzer, HIGH);
88  digitalWrite(ledred, HIGH);
89  servoX.write(107);
90   delay(200);
91  digitalWrite(buzzer, LOW);
92  servoY.write(123);
93  delay(200);
94   digitalWrite(ledred, LOW);
95  digitalWrite(buzzer, HIGH);
96  digitalWrite(ledblu,   HIGH);
97  servoX.write(93);
98  delay(200);
99  digitalWrite(buzzer, LOW);
100   servoY.write(103);
101  delay(200);
102  digitalWrite(ledblu, LOW);
103  digitalWrite(buzzer,   HIGH);
104  digitalWrite(ledgrn, HIGH);
105  servoX.write(80);
106  delay(200);
107   digitalWrite(buzzer, LOW);
108  
109  servoY.write(83);
110  delay(200);
111  servoX.write(93);
112   delay(200);
113  servoY.write(103);
114  delay(500);
115}
116
117void loop() {
118   
119  /* Update all the values */
120  while (i2cRead(0x3B, i2cData, 14));
121  accX   = (int16_t)((i2cData[0] << 8) | i2cData[1]);
122  accY = (int16_t)((i2cData[2] <<   8) | i2cData[3]);
123  accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
124  gyroX   = (int16_t)((i2cData[8] << 8) | i2cData[9]);
125  gyroY = (int16_t)((i2cData[10]   << 8) | i2cData[11]);
126  gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);;
127   systemstateabort();
128  pidcompute();
129  
130}
131 void pidcompute () {
132   double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
133   timer = micros();
134#ifdef RESTRICT_PITCH // Eq. 25 and 26
135  double roll  =   atan2(accY, accZ) * RAD_TO_DEG;
136  double pitch = atan(-accX / sqrt(accY * accY   + accZ * accZ)) * RAD_TO_DEG;
137#else // Eq. 28 and 29
138  double roll  = atan(accY   / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
139  double pitch = atan2(-accX,   accZ) * RAD_TO_DEG;
140#endif
141
142  double gyroXrate = gyroX / 131.0; // Convert   to deg/s
143  double gyroYrate = gyroY / 131.0; // Convert to deg/s
144
145#ifdef   RESTRICT_PITCH
146  // This fixes the transition problem when the accelerometer   angle jumps between -180 and 180 degrees
147  if ((roll < -90 && kalAngleX > 90)   || (roll > 90 && kalAngleX < -90)) {
148    kalmanX.setAngle(roll);
149    kalAngleX   = roll;
150    gyroXangle = roll;
151  } else
152    kalAngleX = kalmanX.getAngle(roll,   gyroXrate, dt); // Calculate the angle using a Kalman filter
153
154  if (abs(kalAngleX)   > 90)
155    gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer   reading
156  kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
157#else
158  //   This fixes the transition problem when the accelerometer angle jumps between -180   and 180 degrees
159  if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY   < -90)) {
160    kalmanY.setAngle(pitch);
161   
162    kalAngleY = pitch;
163    gyroYangle   = pitch;
164  } else
165    kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);   // Calculate the angle using a Kalman filter
166
167  if (abs(kalAngleY) > 90)
168     gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer   reading
169  kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the   angle using a Kalman filter
170#endif
171 
172pwmX = max(pwmX, (desired_angleX -   20)); 
173pwmY = max(pwmY, (desired_angleY - 20)); 
174pwmX = min(pwmX, (desired_angleX   + 20)); 
175pwmY = min(pwmY, (desired_angleY + 20)); 
176
177errorX = kalAngleX   - desired_angleX;
178errorY = kalAngleY - desired_angleY;
179pidX_p = kp*errorX;
180pidX_d   = kd*((errorX - previous_errorX)/dt);
181pidY_p = kp*errorY;
182pidY_d = kd*((errorY   - previous_errorY)/dt);
183pidX_i = ki*errorX*dt;
184previous_errorX = errorX;
185previous_errorY   = errorY;
186PIDX = pidX_p + pidX_i + pidX_d;
187PIDY = pidY_p + pidY_d;
188pwmX   = -1*((PIDX * 5.8) + 90);
189pwmY = ((PIDY * 5.8) + 90);
190servowrite();
191 }
192
193void   servowrite () {
194 servoX.write(pwmX); 
195 servoY.write(pwmY); 
196
197}
198
199   void systemstateabort() {
200 if (kalAngleX > 40 || kalAngleY > 40){
201  digitalWrite(ledgrn,   LOW);
202  digitalWrite(ledblu, HIGH);
203  //digitalWrite(buzzer, HIGH);
204  
205   }
206 }
207

1// 2020 Delta Space Systems //
2 float PIDX, PIDY, errorX, errorY, previous_errorX,  previous_errorY, pwmX, pwmY;
3#include <Wire.h>
4#include <Kalman.h> // Source:  https://github.com/TKJElectronics/KalmanFilter
5#include <Servo.h>
6#define  RESTRICT_PITCH // Comment out to restrict roll to 90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
7Servo  servoY;
8Servo servoX;
9Kalman kalmanX; // Create the Kalman instances
10Kalman  kalmanY;
11
12double accX, accY, accZ;
13double gyroX, gyroY, gyroZ;
14int desired_angleX  = 193;//servoY
15int desired_angleY = -18; // servoX
16double gyroXangle, gyroYangle;  // Angle calculate using the gyro only
17double kalAngleX, kalAngleY; // Calculated  angle using a Kalman filter
18int ledblu=2;
19int ledgrn=5;
20int ledred=6;
21int  buzzer=21;
22uint32_t timer;
23uint8_t i2cData[14]; // Buffer for I2C data
24float  pidX_p=0;
25float pidX_i=0;
26float pidX_d=0;
27float pidY_p=0;
28float pidY_d=0;
29/////////////////PID  CONSTANTS/////////////////
30double kp=0.16;//3.55
31double ki=0;//2.05
32double  kd=0.045;//2.05
33///////////////////////////////////////////////
34int State  = 0;
35void setup() {
36  pinMode(ledblu, OUTPUT);
37  pinMode(ledgrn, OUTPUT);
38  pinMode(ledred, OUTPUT);
39  pinMode(buzzer, OUTPUT);
40 
41  servoY.attach(30);
42   servoX.attach(29);
43  Serial.begin(115200);
44  Wire.begin();
45#if ARDUINO  >= 157
46  Wire.setClock(400000UL); // Set I2C frequency to 400kHz
47#else
48  TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
49#endif
50
51  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
52  i2cData[1]  = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8  KHz sampling
53  i2cData[2] = 0x00; // Set Gyro Full Scale Range to 250deg/s
54  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to 2g
55  while (i2cWrite(0x19,  i2cData, 4, false)); // Write to all four registers at once
56  while (i2cWrite(0x6B,  0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
57
58  while (i2cRead(0x75, i2cData, 1));
59  if (i2cData[0] != 0x68) { // Read "WHO_AM_I"  register
60    Serial.print(F("Error reading sensor"));
61    while (1);
62  }
63  delay(100); // Wait for sensor to stabilize
64  while (i2cRead(0x3B, i2cData,  6));
65  accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
66  accY = (int16_t)((i2cData[2]  << 8) | i2cData[3]);
67  accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
68
69#ifdef  RESTRICT_PITCH // Eq. 25 and 26
70  double roll  = atan2(accY, accZ) * RAD_TO_DEG;
71  double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
72#else  // Eq. 28 and 29
73  double roll  = atan(accY / sqrt(accX * accX + accZ * accZ))  * RAD_TO_DEG;
74  double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
75#endif
76  kalmanX.setAngle(roll); // Set starting angle
77  kalmanY.setAngle(pitch);
78  gyroXangle = roll;
79  gyroYangle = pitch;
80 
81  timer = micros();
82  digitalWrite(ledgrn,  HIGH);
83  servoX.write(93);
84  servoY.write(103);
85  delay(1000);
86  digitalWrite(ledgrn,  LOW);
87  digitalWrite(buzzer, HIGH);
88  digitalWrite(ledred, HIGH);
89  servoX.write(107);
90  delay(200);
91  digitalWrite(buzzer, LOW);
92  servoY.write(123);
93  delay(200);
94  digitalWrite(ledred, LOW);
95  digitalWrite(buzzer, HIGH);
96  digitalWrite(ledblu,  HIGH);
97  servoX.write(93);
98  delay(200);
99  digitalWrite(buzzer, LOW);
100  servoY.write(103);
101  delay(200);
102  digitalWrite(ledblu, LOW);
103  digitalWrite(buzzer,  HIGH);
104  digitalWrite(ledgrn, HIGH);
105  servoX.write(80);
106  delay(200);
107  digitalWrite(buzzer, LOW);
108  
109  servoY.write(83);
110  delay(200);
111  servoX.write(93);
112  delay(200);
113  servoY.write(103);
114  delay(500);
115}
116
117void loop() {
118  
119  /* Update all the values */
120  while (i2cRead(0x3B, i2cData, 14));
121  accX  = (int16_t)((i2cData[0] << 8) | i2cData[1]);
122  accY = (int16_t)((i2cData[2] <<  8) | i2cData[3]);
123  accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
124  gyroX  = (int16_t)((i2cData[8] << 8) | i2cData[9]);
125  gyroY = (int16_t)((i2cData[10]  << 8) | i2cData[11]);
126  gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);;
127  systemstateabort();
128  pidcompute();
129  
130}
131 void pidcompute () {
132  double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
133  timer = micros();
134#ifdef RESTRICT_PITCH // Eq. 25 and 26
135  double roll  =  atan2(accY, accZ) * RAD_TO_DEG;
136  double pitch = atan(-accX / sqrt(accY * accY  + accZ * accZ)) * RAD_TO_DEG;
137#else // Eq. 28 and 29
138  double roll  = atan(accY  / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
139  double pitch = atan2(-accX,  accZ) * RAD_TO_DEG;
140#endif
141
142  double gyroXrate = gyroX / 131.0; // Convert  to deg/s
143  double gyroYrate = gyroY / 131.0; // Convert to deg/s
144
145#ifdef  RESTRICT_PITCH
146  // This fixes the transition problem when the accelerometer  angle jumps between -180 and 180 degrees
147  if ((roll < -90 && kalAngleX > 90)  || (roll > 90 && kalAngleX < -90)) {
148    kalmanX.setAngle(roll);
149    kalAngleX  = roll;
150    gyroXangle = roll;
151  } else
152    kalAngleX = kalmanX.getAngle(roll,  gyroXrate, dt); // Calculate the angle using a Kalman filter
153
154  if (abs(kalAngleX)  > 90)
155    gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer  reading
156  kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
157#else
158  //  This fixes the transition problem when the accelerometer angle jumps between -180  and 180 degrees
159  if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY  < -90)) {
160    kalmanY.setAngle(pitch);
161   
162    kalAngleY = pitch;
163    gyroYangle  = pitch;
164  } else
165    kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);  // Calculate the angle using a Kalman filter
166
167  if (abs(kalAngleY) > 90)
168    gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer  reading
169  kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the  angle using a Kalman filter
170#endif
171 
172pwmX = max(pwmX, (desired_angleX -  20)); 
173pwmY = max(pwmY, (desired_angleY - 20)); 
174pwmX = min(pwmX, (desired_angleX  + 20)); 
175pwmY = min(pwmY, (desired_angleY + 20)); 
176
177errorX = kalAngleX  - desired_angleX;
178errorY = kalAngleY - desired_angleY;
179pidX_p = kp*errorX;
180pidX_d  = kd*((errorX - previous_errorX)/dt);
181pidY_p = kp*errorY;
182pidY_d = kd*((errorY  - previous_errorY)/dt);
183pidX_i = ki*errorX*dt;
184previous_errorX = errorX;
185previous_errorY  = errorY;
186PIDX = pidX_p + pidX_i + pidX_d;
187PIDY = pidY_p + pidY_d;
188pwmX  = -1*((PIDX * 5.8) + 90);
189pwmY = ((PIDY * 5.8) + 90);
190servowrite();
191 }
192
193void  servowrite () {
194 servoX.write(pwmX); 
195 servoY.write(pwmY); 
196
197}
198
199  void systemstateabort() {
200 if (kalAngleX > 40 || kalAngleY > 40){
201  digitalWrite(ledgrn,  LOW);
202  digitalWrite(ledblu, HIGH);
203  //digitalWrite(buzzer, HIGH);
204  
205  }
206 }
207

1/* Delta Space Systems
2      Version 1.2
3    May, 8th 2020*/
4
5#include
6  <SD.h>
7#include <SPI.h>
8#include <Servo.h>
9#include<Wire.h>
10
11//#include
12  <KalmanFilter.h>
13const int MPU=0x68; 
14int16_t AcX,AcY,AcZ,Tmp,GyX,GyroY,GyroZ;
15
16  double PIDX, PIDY, errorX, errorY, previous_errorX, previous_errorY, pwmX, pwmY,
17  gyroXX, gyroYY;
18 
19int desired_angleX = 0;//servoY
20int desired_angleY =
21  0;//servoX
22
23int servoX_offset = 100;
24int servoY_offset = 100;
25
26int
27  servoXstart = 80;
28int servoYstart = 80;
29
30float servo_gear_ratio = 5.8;
31
32
33double
34  accAngleX;
35double accAngleY;
36double yaw;
37double GyroX;
38double gyroAngleX;
39double
40  gyroAngleY;
41double pitch;
42
43int ledblu = 2;    // LED connected to digital
44  pin 9
45int ledgrn = 5;    // LED connected to digital pin 9
46int ledred = 6;
47    // LED connected to digital pin 9
48int mosfet = 24;
49
50Servo servoX;
51Servo
52  servoY;
53
54int pyro1 = 24;
55int buzzer = 21;
56
57double dt, currentTime,
58  previousTime;
59
60float pidX_p = 0;
61float pidX_i = 0;
62float pidX_d = 0;
63float
64  pidY_p = 0;
65float pidY_i = 0;
66float pidY_d = 0;
67
68int pos;
69
70double
71  kp = 0.15;
72double ki = 0.0;
73double kd = 0.0;
74
75int state = 0;
76//KalmanFilter
77  kalman(0.001, 0.003, 0.03);
78
79void setup(){
80  Wire.begin();
81  Wire.beginTransmission(MPU);
82
83  Wire.write(0x6B); 
84  Wire.write(0);    
85  Wire.endTransmission(true);
86
87  Serial.begin(9600);
88  servoX.attach(29);
89  servoY.attach(30);
90  pinMode(mosfet,
91  OUTPUT);
92   pinMode(ledblu, OUTPUT);
93  pinMode(ledgrn, OUTPUT);
94  pinMode(ledred,
95  OUTPUT);
96  pinMode(buzzer, OUTPUT);
97  pinMode(pyro1, OUTPUT);
98 startup();
99
100
101  
102}
103void loop() {
104  
105
106  previousTime = currentTime;        
107  currentTime
108  = millis();            
109  dt = (currentTime - previousTime) / 1000; 
110 
111
112  accAngleX = (atan(AcY / sqrt(pow(AcX, 2) + pow(AcZ, 2))) * 180 / PI) - 0.58; //
113  AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details
114
115  accAngleY = (atan(-1 * AcX / sqrt(pow(AcY, 2) + pow(AcZ, 2))) * 180 / PI) + 1.58;
116  // 
117 
118  Wire.beginTransmission(MPU);
119  Wire.write(0x3B);  
120  Wire.endTransmission(false);
121
122  Wire.requestFrom(MPU,12,true);
123  AcX=Wire.read()<<8|Wire.read();    
124  AcY=Wire.read()<<8|Wire.read();
125  
126  AcZ=Wire.read()<<8|Wire.read();  
127  GyroX=Wire.read()<<8|Wire.read();
128  
129  GyroY=Wire.read()<<8|Wire.read();  
130  GyroZ=Wire.read()<<8|Wire.read();
131  
132  datadump();
133  launchdetect();
134  
135}
136void accel_degrees () {
137
138  double prevgyroX = GyroZ;
139  double prevgyroY = GyroY;
140  
141  //converting
142  angular acceleration to degrees
143  gyroAngleX +=  (((GyroZ + prevgyroX) / 2)*
144  dt); // deg/s * s = deg
145  gyroAngleY +=  (((GyroY + prevgyroY) / 2)* dt);
146
147  
148  //complimentary filter
149  double OrientationX = 0.94 * gyroAngleX + 0.06
150  * accAngleX;
151  double OrientationY = 0.94 * gyroAngleY + 0.06 * accAngleY;
152
153  
154  //divided by 32.8 as recommended by the datasheet
155  pitch = OrientationX
156  / 32.8;
157  yaw = OrientationY / 32.8;
158  pidcompute();
159}
160
161void servowrite()
162  {
163
164 servoX.write(pwmX);
165 servoY.write(pwmY); 
166
167}
168void pidcompute
169  () {
170  
171previous_errorX = errorX;
172previous_errorY = errorY; 
173
174errorX
175  = pitch - desired_angleX;
176errorY = yaw - desired_angleY;
177
178//Defining "P"
179  
180pidX_p = kp*errorX;
181pidY_p = kp*errorY;
182
183//Defining "D"
184pidX_d
185  = kd*((errorX - previous_errorX)/dt);
186pidY_d = kd*((errorY - previous_errorY)/dt);
187
188//Defining
189  "I"
190pidX_i = ki * (pidX_i + errorX * dt);
191pidY_i = ki * (pidY_i + errorY
192  * dt);
193
194PIDX = pidX_p + pidX_i + pidX_d;
195PIDY = pidY_p + pidY_i + pidY_d;
196
197pwmX
198  = ((PIDX * servo_gear_ratio) + servoX_offset);
199pwmY = ((PIDY * servo_gear_ratio)
200  + servoY_offset);
201servowrite();
202
203}
204void startup () {
205  digitalWrite(ledgrn,
206  HIGH);
207  servoX.write(servoXstart);
208  servoY.write(servoYstart);
209  delay(1000);
210
211  digitalWrite(ledgrn, LOW);
212  digitalWrite(buzzer, HIGH);
213  digitalWrite(ledred,
214  HIGH);
215  servoX.write(servoXstart + 20);
216  delay(200);
217  digitalWrite(buzzer,
218  LOW);
219  servoY.write(servoYstart + 20);
220  delay(200);
221  digitalWrite(ledred,
222  LOW);
223  digitalWrite(buzzer, HIGH);
224  digitalWrite(ledblu, HIGH);
225  servoX.write(servoXstart);
226
227  delay(200);
228  digitalWrite(buzzer, LOW);
229  servoY.write(servoYstart);
230
231  delay(200);
232  digitalWrite(ledblu, LOW);
233  digitalWrite(buzzer, HIGH);
234
235  digitalWrite(ledgrn, HIGH);
236  servoX.write(servoXstart - 20);
237  delay(200);
238
239  digitalWrite(buzzer, LOW);
240  
241  servoY.write(servoYstart - 20);
242  delay(200);
243
244  servoX.write(servoXstart);
245  delay(200);
246  servoY.write(servoYstart);
247
248  delay(500);
249 }
250void launchdetect () {
251   if (AcX > 17000) {
252  state
253  = 1;
254 }
255 if (state == 1) {
256  accel_degrees();
257 }
258}
259void datadump
260  () {
261Serial.println(AcX);
262Serial.print(AcZ);
263}
264

1/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
2
3
4  This software may be distributed and modified under the terms of the GNU
5 General
6  Public License version 2 (GPL2) as published by the Free Software
7 Foundation
8  and appearing in the file GPL2.TXT included in the packaging of
9 this file. Please
10  note that GPL2 Section 2[b] requires that all works based
11 on this software must
12  also be made publicly available under the terms of
13 the GPL2 ("Copyleft").
14
15
16  Contact information
17 -------------------
18
19 Kristian Lauszus, TKJ Electronics
20
21  Web      :  http://www.tkjelectronics.com
22 e-mail   :  kristianl@tkjelectronics.com
23
24  */
25
26const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
27const
28  uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication
29
30uint8_t
31  i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
32  return i2cWrite(registerAddress,
33  &data, 1, sendStop); // Returns 0 on success
34}
35
36uint8_t i2cWrite(uint8_t
37  registerAddress, uint8_t *data, uint8_t length, bool sendStop) {
38  Wire.beginTransmission(IMUAddress);
39
40  Wire.write(registerAddress);
41  Wire.write(data, length);
42  uint8_t rcode
43  = Wire.endTransmission(sendStop); // Returns 0 on success
44  if (rcode) {
45
46    Serial.print(F("i2cWrite failed: "));
47    Serial.println(rcode);
48  }
49
50  return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
51}
52
53uint8_t
54  i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
55  uint32_t
56  timeOutTimer;
57  Wire.beginTransmission(IMUAddress);
58  Wire.write(registerAddress);
59
60  uint8_t rcode = Wire.endTransmission(false); // Don't release the bus
61  if
62  (rcode) {
63    Serial.print(F("i2cRead failed: "));
64    Serial.println(rcode);
65
66    return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
67
68  }
69  Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated
70  start and then release the bus after reading
71  for (uint8_t i = 0; i < nbytes;
72  i++) {
73    if (Wire.available())
74      data[i] = Wire.read();
75    else
76  {
77      timeOutTimer = micros();
78      while (((micros() - timeOutTimer) <
79  I2C_TIMEOUT) && !Wire.available());
80      if (Wire.available())
81        data[i]
82  = Wire.read();
83      else {
84        Serial.println(F("i2cRead timeout"));
85
86        return 5; // This error value is not already taken by endTransmission
87
88      }
89    }
90  }
91  return 0; // Success
92} 
93

1/* Delta Space Systems
2      Version 1.2
3    May, 8th 2020*/
4
5#include  <SD.h>
6#include <SPI.h>
7#include <Servo.h>
8#include<Wire.h>
9
10//#include  <KalmanFilter.h>
11const int MPU=0x68; 
12int16_t AcX,AcY,AcZ,Tmp,GyX,GyroY,GyroZ;
13  double PIDX, PIDY, errorX, errorY, previous_errorX, previous_errorY, pwmX, pwmY,  gyroXX, gyroYY;
14 
15int desired_angleX = 0;//servoY
16int desired_angleY =  0;//servoX
17
18int servoX_offset = 100;
19int servoY_offset = 100;
20
21int  servoXstart = 80;
22int servoYstart = 80;
23
24float servo_gear_ratio = 5.8;
25
26
27double  accAngleX;
28double accAngleY;
29double yaw;
30double GyroX;
31double gyroAngleX;
32double  gyroAngleY;
33double pitch;
34
35int ledblu = 2;    // LED connected to digital  pin 9
36int ledgrn = 5;    // LED connected to digital pin 9
37int ledred = 6;    // LED connected to digital pin 9
38int mosfet = 24;
39
40Servo servoX;
41Servo  servoY;
42
43int pyro1 = 24;
44int buzzer = 21;
45
46double dt, currentTime,  previousTime;
47
48float pidX_p = 0;
49float pidX_i = 0;
50float pidX_d = 0;
51float  pidY_p = 0;
52float pidY_i = 0;
53float pidY_d = 0;
54
55int pos;
56
57double  kp = 0.15;
58double ki = 0.0;
59double kd = 0.0;
60
61int state = 0;
62//KalmanFilter  kalman(0.001, 0.003, 0.03);
63
64void setup(){
65  Wire.begin();
66  Wire.beginTransmission(MPU);
67  Wire.write(0x6B); 
68  Wire.write(0);    
69  Wire.endTransmission(true);
70  Serial.begin(9600);
71  servoX.attach(29);
72  servoY.attach(30);
73  pinMode(mosfet,  OUTPUT);
74   pinMode(ledblu, OUTPUT);
75  pinMode(ledgrn, OUTPUT);
76  pinMode(ledred,  OUTPUT);
77  pinMode(buzzer, OUTPUT);
78  pinMode(pyro1, OUTPUT);
79 startup();
80
81  
82}
83void loop() {
84  
85
86  previousTime = currentTime;        
87  currentTime  = millis();            
88  dt = (currentTime - previousTime) / 1000; 
89 
90  accAngleX = (atan(AcY / sqrt(pow(AcX, 2) + pow(AcZ, 2))) * 180 / PI) - 0.58; //  AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details
91  accAngleY = (atan(-1 * AcX / sqrt(pow(AcY, 2) + pow(AcZ, 2))) * 180 / PI) + 1.58;  // 
92 
93  Wire.beginTransmission(MPU);
94  Wire.write(0x3B);  
95  Wire.endTransmission(false);
96  Wire.requestFrom(MPU,12,true);
97  AcX=Wire.read()<<8|Wire.read();    
98  AcY=Wire.read()<<8|Wire.read();  
99  AcZ=Wire.read()<<8|Wire.read();  
100  GyroX=Wire.read()<<8|Wire.read();  
101  GyroY=Wire.read()<<8|Wire.read();  
102  GyroZ=Wire.read()<<8|Wire.read();  
103  datadump();
104  launchdetect();
105  
106}
107void accel_degrees () {
108  double prevgyroX = GyroZ;
109  double prevgyroY = GyroY;
110  
111  //converting  angular acceleration to degrees
112  gyroAngleX +=  (((GyroZ + prevgyroX) / 2)*  dt); // deg/s * s = deg
113  gyroAngleY +=  (((GyroY + prevgyroY) / 2)* dt);
114  
115  //complimentary filter
116  double OrientationX = 0.94 * gyroAngleX + 0.06  * accAngleX;
117  double OrientationY = 0.94 * gyroAngleY + 0.06 * accAngleY;
118  
119  //divided by 32.8 as recommended by the datasheet
120  pitch = OrientationX  / 32.8;
121  yaw = OrientationY / 32.8;
122  pidcompute();
123}
124
125void servowrite()  {
126
127 servoX.write(pwmX);
128 servoY.write(pwmY); 
129
130}
131void pidcompute  () {
132  
133previous_errorX = errorX;
134previous_errorY = errorY; 
135
136errorX  = pitch - desired_angleX;
137errorY = yaw - desired_angleY;
138
139//Defining "P"  
140pidX_p = kp*errorX;
141pidY_p = kp*errorY;
142
143//Defining "D"
144pidX_d  = kd*((errorX - previous_errorX)/dt);
145pidY_d = kd*((errorY - previous_errorY)/dt);
146
147//Defining  "I"
148pidX_i = ki * (pidX_i + errorX * dt);
149pidY_i = ki * (pidY_i + errorY  * dt);
150
151PIDX = pidX_p + pidX_i + pidX_d;
152PIDY = pidY_p + pidY_i + pidY_d;
153
154pwmX  = ((PIDX * servo_gear_ratio) + servoX_offset);
155pwmY = ((PIDY * servo_gear_ratio)  + servoY_offset);
156servowrite();
157
158}
159void startup () {
160  digitalWrite(ledgrn,  HIGH);
161  servoX.write(servoXstart);
162  servoY.write(servoYstart);
163  delay(1000);
164  digitalWrite(ledgrn, LOW);
165  digitalWrite(buzzer, HIGH);
166  digitalWrite(ledred,  HIGH);
167  servoX.write(servoXstart + 20);
168  delay(200);
169  digitalWrite(buzzer,  LOW);
170  servoY.write(servoYstart + 20);
171  delay(200);
172  digitalWrite(ledred,  LOW);
173  digitalWrite(buzzer, HIGH);
174  digitalWrite(ledblu, HIGH);
175  servoX.write(servoXstart);
176  delay(200);
177  digitalWrite(buzzer, LOW);
178  servoY.write(servoYstart);
179  delay(200);
180  digitalWrite(ledblu, LOW);
181  digitalWrite(buzzer, HIGH);
182  digitalWrite(ledgrn, HIGH);
183  servoX.write(servoXstart - 20);
184  delay(200);
185  digitalWrite(buzzer, LOW);
186  
187  servoY.write(servoYstart - 20);
188  delay(200);
189  servoX.write(servoXstart);
190  delay(200);
191  servoY.write(servoYstart);
192  delay(500);
193 }
194void launchdetect () {
195   if (AcX > 17000) {
196  state  = 1;
197 }
198 if (state == 1) {
199  accel_degrees();
200 }
201}
202void datadump  () {
203Serial.println(AcX);
204Serial.print(AcZ);
205}
206

1/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
2
3  This software may be distributed and modified under the terms of the GNU
4 General  Public License version 2 (GPL2) as published by the Free Software
5 Foundation  and appearing in the file GPL2.TXT included in the packaging of
6 this file. Please  note that GPL2 Section 2[b] requires that all works based
7 on this software must  also be made publicly available under the terms of
8 the GPL2 ("Copyleft").
9
10  Contact information
11 -------------------
12
13 Kristian Lauszus, TKJ Electronics
14  Web      :  http://www.tkjelectronics.com
15 e-mail   :  kristianl@tkjelectronics.com
16  */
17
18const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
19const  uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication
20
21uint8_t  i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
22  return i2cWrite(registerAddress,  &data, 1, sendStop); // Returns 0 on success
23}
24
25uint8_t i2cWrite(uint8_t  registerAddress, uint8_t *data, uint8_t length, bool sendStop) {
26  Wire.beginTransmission(IMUAddress);
27  Wire.write(registerAddress);
28  Wire.write(data, length);
29  uint8_t rcode  = Wire.endTransmission(sendStop); // Returns 0 on success
30  if (rcode) {
31    Serial.print(F("i2cWrite failed: "));
32    Serial.println(rcode);
33  }
34  return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
35}
36
37uint8_t  i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
38  uint32_t  timeOutTimer;
39  Wire.beginTransmission(IMUAddress);
40  Wire.write(registerAddress);
41  uint8_t rcode = Wire.endTransmission(false); // Don't release the bus
42  if  (rcode) {
43    Serial.print(F("i2cRead failed: "));
44    Serial.println(rcode);
45    return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
46  }
47  Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated  start and then release the bus after reading
48  for (uint8_t i = 0; i < nbytes;  i++) {
49    if (Wire.available())
50      data[i] = Wire.read();
51    else  {
52      timeOutTimer = micros();
53      while (((micros() - timeOutTimer) <  I2C_TIMEOUT) && !Wire.available());
54      if (Wire.available())
55        data[i]  = Wire.read();
56      else {
57        Serial.println(F("i2cRead timeout"));
58        return 5; // This error value is not already taken by endTransmission
59      }
60    }
61  }
62  return 0; // Success
63} 
64