-
Notifications
You must be signed in to change notification settings - Fork 2
/
Motors.ino
163 lines (133 loc) · 5.49 KB
/
Motors.ino
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
/*
Motors
1 2 PIN3 PIN5
\ / \ /
X X
/ \ / \
3 4 PIN6 PIN9
*/
#include <Servo.h>
Servo MOTOR1;
Servo MOTOR2;
Servo MOTOR3;
Servo MOTOR4;
void Motor_Arm()
{
MOTOR1.attach(MOTORpin1);
MOTOR2.attach(MOTORpin2);
MOTOR3.attach(MOTORpin3);
MOTOR4.attach(MOTORpin4);
MOTOR1.writeMicroseconds(750);
MOTOR2.writeMicroseconds(750);
MOTOR3.writeMicroseconds(750);
MOTOR4.writeMicroseconds(750);
}
double total_error_yaw= 0;
double total_error_pitch= 0;
double total_error_roll= 0;
double old_error_yaw = 0;
double old_error_pitch = 0;
double old_error_roll= 0;
double total_error_yaw_angvel= 0;
double total_error_pitch_angvel= 0;
double total_error_roll_angvel= 0;
double old_error_yaw_angvel = 0;
double old_error_pitch_angvel = 0;
double old_error_roll_angvel = 0;
void Motor_Update(int mode)
{
////////////////////////////////First PID/////////////////////////////////
double error_yaw = desired_yaw-real_yaw;
double error_pitch= desired_pitch-real_pitch;
double error_roll=desired_roll-real_roll;
if (desired_thr>100)//when throttle is bigger than 100//
{
total_error_yaw += error_yaw*loop_timer;
total_error_pitch += error_pitch*loop_timer;
total_error_roll += error_roll*loop_timer;
}
double error_change_yaw = (error_yaw- old_error_yaw)/loop_timer;
double error_change_pitch = (error_pitch- old_error_pitch)/loop_timer;
double error_change_roll = (error_roll- old_error_roll)/loop_timer;
double desired_yaw_angvel=calculate_pid(kp_stab_yaw, ki_stab_yaw, kd_stab_yaw, error_yaw, total_error_yaw, error_change_yaw);
double desired_pitch_angvel=calculate_pid(kp_stab_pitch, ki_stab_pitch, kd_stab_pitch, error_pitch, total_error_pitch, error_change_pitch);
double desired_roll_angvel=calculate_pid(kp_stab_roll, ki_stab_roll, kd_stab_roll, error_roll, total_error_roll, error_change_roll);
////////////////////////////////Second PID/////////////////////////////////
double error_yaw_angvel = desired_yaw_angvel - real_yaw_angvel;
double error_pitch_angvel = desired_pitch_angvel - real_pitch_angvel;
double error_roll_angvel =desired_roll_angvel - real_roll_angvel;
if (desired_thr>100)//when throttle is bigger than 100//
{
total_error_yaw_angvel += error_yaw_angvel*loop_timer;
total_error_pitch_angvel += error_pitch_angvel*loop_timer;
total_error_roll_angvel += error_roll_angvel*loop_timer;
}
double error_change_yaw_angvel = (error_yaw_angvel- old_error_yaw_angvel)/loop_timer;
double error_change_pitch_angvel = (error_pitch_angvel- old_error_pitch_angvel)/loop_timer;
double error_change_roll_angvel = (error_roll_angvel- old_error_roll_angvel)/loop_timer;
double output_motor_yaw=calculate_pid(kp_motor_yaw, ki_motor_yaw, kd_motor_yaw, error_yaw_angvel, total_error_yaw_angvel, error_change_yaw_angvel);
double output_motor_pitch=calculate_pid(kp_motor_pitch, ki_motor_pitch, kd_motor_pitch, error_pitch_angvel, total_error_pitch_angvel, error_change_pitch_angvel);
double output_motor_roll=calculate_pid(kp_motor_roll, ki_motor_roll, kd_motor_roll, error_roll_angvel, total_error_roll_angvel, error_change_roll_angvel);
// Serial.print(output_motor_yaw); Serial.print("\t"); Serial.print(output_motor_pitch); Serial.print("\t");Serial.println(output_motor_roll);
////////////////////////////////Write Outputs to Motors/////////////////////////////////
if (desired_thr>100)//when throttle is bigger than 100//
{
fl_output= (desired_thr - output_motor_roll - output_motor_pitch- output_motor_yaw);
bl_output=(desired_thr - output_motor_roll + output_motor_pitch+ output_motor_yaw);
fr_output=(desired_thr + output_motor_roll - output_motor_pitch+ output_motor_yaw);
br_output=(desired_thr + output_motor_roll + output_motor_pitch- output_motor_yaw);
fl_output= setlimit(fl_output,1000,0);
bl_output= setlimit(bl_output,1000,0);
fr_output=setlimit(fr_output,1000,0);
br_output=setlimit(br_output,1000,0);
MOTOR1.writeMicroseconds(map(fl_output, 0, 1023, 700, 2000));
MOTOR2.writeMicroseconds(map(fr_output, 0, 1023, 700, 2000));
MOTOR3.writeMicroseconds(map(bl_output, 0, 1023, 700, 2000));
MOTOR4.writeMicroseconds(map(br_output, 0, 1023, 700, 2000));
}
else
{
fl_output=0;
bl_output=0;
fr_output=0;
br_output=0;
MOTOR1.writeMicroseconds(750);
MOTOR2.writeMicroseconds(750);
MOTOR3.writeMicroseconds(750);
MOTOR4.writeMicroseconds(750);
}
/////////////For next loop- ypr//////////////////
old_real_yaw=real_yaw;
old_real_pitch=real_pitch;
old_real_roll=real_roll;
old_error_yaw = error_yaw;
old_error_pitch = error_pitch;
old_error_roll = error_roll;
/////////////For next loop- ypr-angular velocity//////////////////
old_real_yaw_angvel=real_yaw_angvel;
old_real_pitch_angvel=real_pitch_angvel;
old_real_roll_angvel=real_roll_angvel;
old_error_yaw_angvel = error_yaw_angvel;
old_error_pitch_angvel = error_pitch_angvel;
old_error_roll_angvel = error_roll_angvel;
}
double calculate_pid(double kp, double ki, double kd, double error, double total_error,double error_change )
{
double pid=kp*error + ki*total_error + kd*error_change ;
return pid;
}
double setlimit(double number, double maxlimit, double minlimit)
{
if(number>maxlimit)
{
return maxlimit;
}
else if (number<minlimit)
{
return minlimit;
}
else
{
return number;
}
}