-
Notifications
You must be signed in to change notification settings - Fork 0
/
Root
180 lines (150 loc) · 6.86 KB
/
Root
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
//WOZ FC/OS - July 23rd 2020 - Watfly Inc.
//Controlling Atlas from behind the green curtain
// 1.0 Library Definitions:
#include <Wire.h> //I2C Library
#include <DSMRX.h> //RC Library
#include <PID_v1.h> //PID Library
#include <Servo.h> //PWM/Servo Library
#include <Adafruit_Sensor.h> //Standard blackbox adafruit sensor lib
#include <Adafruit_BNO055.h> //BNO055 is the IMU IC
#include <utility/imumaths.h> //this calls the vector.h library
// 1.1 Control Definitions:
double max_pitch = 90.0, max_roll = 90.0, max_yaw = 90.0;
// 1.2 RC Definitions:
double throttle_rc, throttle_setpoint, pitch_rc, yaw_rc, roll_rc;
DSM2048 rx;
static const uint8_t CHANNELS = 8;
// 1.3 PID Definitions:
//Input from IMU, Setpoint from RC
double pitch_setpoint, pitch_input, pitch_output; //PID Pitch Var
double yaw_setpoint, yaw_input, yaw_output; //PID Yaw Var
double roll_setpoint, roll_input, roll_output; //PID Roll Var
double pitch_p = 4.5, pitch_i = 0.3, pitch_d = 0.002; //Pitch PID gains
double yaw_p = 3.8, yaw_i = 0.22, yaw_d = 0.02; //Pitch PID gains
double roll_p = 6, roll_i = 0.12, roll_d = 0.002; //Pitch PID gains
float pid, pwmLeft, pwmRight, error, previous_error;
float pid_p = 0, pid_i = 0, pid_d = 0;
PID pitch_PID(&pitch_input, &pitch_output, &pitch_setpoint, pitch_p, pitch_i, pitch_d, DIRECT); // Pitch PID
PID yaw_PID(&yaw_input, &yaw_output, &yaw_setpoint, yaw_p, yaw_i, yaw_d, DIRECT); // Yaw PID
PID roll_PID(&roll_input, &roll_output, &roll_setpoint, yaw_p, yaw_i, yaw_d, DIRECT); // Roll PID
// 1.4 IMU Definitions:
Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28); //Default BNO055 address
double pitch_att, yaw_att, roll_att;
// 1.5 Servo/PWM Definitions:
Servo rflap, lflap, rmotor, lmotor;
double rflap_output, lflap_output, rmotor_output, lmotor_output;
//RC this separate function opens and maintains serial comms with receiver, will run perpetually in parallel
void serialEvent1(void)
{
while(Serial1.available()) {
rx.handleSerialEvent(Serial1.read(),micros()); //RC Receiver Data Pin into RX1 (Mega)
}
}
void setup(void)
{
Serial.begin(115200);
Serial1.begin(115000); //For RC Receiver Comms
//Servo/PWM Setup:
rflap.attach(8,0,2400), lflap.attach(9,0,2400), rmotor.attach(4,1000,2000), lmotor.attach(5,1000,2000);
rflap.writeMicroseconds(1000), lflap.writeMicroseconds(1000), rmotor.writeMicroseconds(1000), lmotor.writeMicroseconds(1000);
//PID Setup
pitch_PID.SetMode(AUTOMATIC), yaw_PID.SetMode(AUTOMATIC), roll_PID.SetMode(AUTOMATIC);//Turn the PIDs on
pitch_PID.SetOutputLimits(-255,255), yaw_PID.SetOutputLimits(-255,255), roll_PID.SetOutputLimits(-255,255);
pitch_PID.SetSampleTime(10), yaw_PID.SetSampleTime(10), roll_PID.SetSampleTime(10);
//IMU Setup
Serial.println("IMU Test"); Serial.println("");
/* Initialise the sensor */
if (!bno.begin())
{
/* There was a problem detecting the BNO055 ... check your connections */
Serial.print("No IMU detected");
while (1);
}
else { Serial.print("Test succesful");}
delay(1000); //Give time for sensor to setup
// Serial.println("Type in 420 to initialize."); //User must confirm program is ready to initialize. IMU will tare.
// while(Serial.available() != 420) {}
}
void loop(void)
{
//RC Elements:
if (rx.timedOut(micros())) {
Serial.println("*RC Timed Out*");
}
else if (rx.gotNewFrame())
{
uint16_t values[CHANNELS];
rx.getChannelValues(values, CHANNELS);
throttle_rc = values[0];
roll_rc = values[1];
pitch_rc = values[2];
yaw_rc = values [3];
//Loop below is for print purposes, can comment on release version
// for (int k=0; k<CHANNELS; ++k)
// {
// Serial.print("Ch. "), Serial.print(k+1), Serial.print(": "), Serial.print(values[k]), Serial.print(" ");
// }
// serial.print("Fade count = "), Serial.println(rx.getFadeCount());
// delay(5); //Likely don't need extra delay with all the code below
}
//RC Inputs to Setpoints + Normalization
pitch_setpoint = (pitch_rc - 1500) * (max_pitch / 500.0); //From PWM to Degrees to enter PID
roll_setpoint = (roll_rc - 1500) * (max_roll / 500.0); //From PWM to Degrees to enter PID
yaw_setpoint = (yaw_rc - 1500) * (max_yaw / 500.0); //**WIP**
// IMU Elements:
sensors_event_t event;
bno.getEvent(&event);
/* Board layout:
+----------+
| *| RST PITCH ROLL HEADING
ADR |* *| SCL
INT |* *| SDA ^ /->
PS1 |* *| GND | |
PS0 |* *| 3VO Y Z--> \-X
| *| VIN
+----------+
*/
imu::Quaternion q = bno.getQuat(); //This is from Adafruit sample code
q.normalize();
float temp = q.x(); q.x() = -q.y(); q.y() = temp; //Flipping axes
q.z() = -q.z();
imu::Vector<3> euler = q.toEuler();
yaw_att = (-180/M_PI) * euler.x();
pitch_att = (-180/M_PI) * euler.y();
roll_att = (-180/M_PI) * euler.z();
//Below is just prints, can comment in final ver.
// Serial.print(F("Orientation: "));
// Serial.print(F("Yaw: "));
// Serial.print(yaw_att); // heading, nose-right is positive, z-axis points up
// Serial.print(F(" Pitch: "));
// Serial.print(pitch_att); // roll, rightwing-up is positive, y-axis points forward
// Serial.print(F(" Roll: "));
// Serial.print(roll_att); // pitch, nose-down is positive, x-axis points right
// Serial.println(F(""));
//PID elements:
pitch_input = pitch_att, yaw_input = yaw_att, roll_input = roll_att;
pitch_PID.Compute(), yaw_PID.Compute(), roll_PID.Compute();
// Serial.print("Pitch: "), Serial.print(pitch_att);
// Serial.print(" | Pitch Output: "), Serial.println(pitch_output);
//Normalize PID output (from degrees to normalized values)
yaw_output = yaw_output / 255.00; //Yaw in -1.0 to 1.0
pitch_output = pitch_output / 255.00; //Pitch in -1.0 to 1.0
roll_output = roll_output / 255.00; //Roll in -1.0 to 1.0
throttle_setpoint = (throttle_rc - 1000.00) / 1000.00; //Throttle in 0.0 to 1.0
//Mixer / Mixing:
rflap_output = constrain(( (yaw_output * -1 + 0) * 0.75 + (pitch_output * 1 + 0) * 0.75)* 1 + 0, -1, 1);
lflap_output = constrain(( (yaw_output * -1 + 0) * 0.75 + (pitch_output * -1 + 0) * 0.75)* 1 + 0, -1, 1);
rmotor_output = constrain(( (roll_output * 1) + (throttle_setpoint * 1)), 0 , 1);
lmotor_output =constrain(( (roll_output * -1) + (throttle_setpoint * 1)), 0, 1);
//In PX4, roll, pitch and yaw inputs are expected to range from -1.0 to 1.0
//While thrust input ranges from 0.0 to 1.0
// Serial.print("Pitch: "), Serial.print(pitch_att);
// Serial.print(" | rmotor: "), Serial.print(throttle + pitch_output);
// Serial.print(" | lmotor: "), Serial.println(throttle - pitch_output);
//Peripheral Outputs
rflap.writeMicroseconds((rflap_output * 1200.00) + 1200.00);
lflap.writeMicroseconds((lflap_output * 1200.00) + 1200.00);
rmotor.writeMicroseconds(1000.00 + (rmotor_output * 1000.00));
lmotor.writeMicroseconds(1000.00 + (lmotor_output * 1000.00));
delay(100);
}