KATAPANG
/
PID
bla
main.cpp@0:1d81f3296468, 2018-08-29 (annotated)
- Committer:
- arayhan878
- Date:
- Wed Aug 29 15:59:31 2018 +0000
- Revision:
- 0:1d81f3296468
- Child:
- 1:679511199e94
ik
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
arayhan878 | 0:1d81f3296468 | 1 | #include "mbed.h" |
arayhan878 | 0:1d81f3296468 | 2 | |
arayhan878 | 0:1d81f3296468 | 3 | ///////////////////////PID/////////////////////// |
arayhan878 | 0:1d81f3296468 | 4 | int out, e, integral, derivative, preError, output; |
arayhan878 | 0:1d81f3296468 | 5 | float Dt = 1; |
arayhan878 | 0:1d81f3296468 | 6 | float Kp = 1; |
arayhan878 | 0:1d81f3296468 | 7 | float Ki = 0; |
arayhan878 | 0:1d81f3296468 | 8 | float Kd = 2; |
arayhan878 | 0:1d81f3296468 | 9 | int setpoint = 0; |
arayhan878 | 0:1d81f3296468 | 10 | ///////////////////////PID//////////////////////// |
arayhan878 | 0:1d81f3296468 | 11 | |
arayhan878 | 0:1d81f3296468 | 12 | Serial pc(USBTX, USBRX); |
arayhan878 | 0:1d81f3296468 | 13 | Serial bt(PA_11, PA_12); |
arayhan878 | 0:1d81f3296468 | 14 | |
arayhan878 | 0:1d81f3296468 | 15 | int co, ch, ch1; |
arayhan878 | 0:1d81f3296468 | 16 | |
arayhan878 | 0:1d81f3296468 | 17 | DigitalOut dir1(PA_10); |
arayhan878 | 0:1d81f3296468 | 18 | DigitalOut dir2(PB_3); |
arayhan878 | 0:1d81f3296468 | 19 | DigitalOut dir3(PA_7); |
arayhan878 | 0:1d81f3296468 | 20 | DigitalOut dir4(PB_6); |
arayhan878 | 0:1d81f3296468 | 21 | DigitalOut dir5(PC_7); |
arayhan878 | 0:1d81f3296468 | 22 | DigitalOut dir6(PA_9); |
arayhan878 | 0:1d81f3296468 | 23 | PwmOut pwm1(PA_8); |
arayhan878 | 0:1d81f3296468 | 24 | PwmOut pwm2(PB_10); |
arayhan878 | 0:1d81f3296468 | 25 | PwmOut pwm3(PB_4); |
arayhan878 | 0:1d81f3296468 | 26 | |
arayhan878 | 0:1d81f3296468 | 27 | int count = 0; |
arayhan878 | 0:1d81f3296468 | 28 | int pos1 = 0; |
arayhan878 | 0:1d81f3296468 | 29 | int pos2 = 0; |
arayhan878 | 0:1d81f3296468 | 30 | |
arayhan878 | 0:1d81f3296468 | 31 | float roda1, roda2, roda3; |
arayhan878 | 0:1d81f3296468 | 32 | float pw1,pw2,pw3; |
arayhan878 | 0:1d81f3296468 | 33 | int pwma1, pwma2, pwma3; |
arayhan878 | 0:1d81f3296468 | 34 | int abs_pwm1, abs_pwm2, abs_pwm3; |
arayhan878 | 0:1d81f3296468 | 35 | double dx, dy, dz; |
arayhan878 | 0:1d81f3296468 | 36 | |
arayhan878 | 0:1d81f3296468 | 37 | void atur(double x, double y, double the) |
arayhan878 | 0:1d81f3296468 | 38 | { |
arayhan878 | 0:1d81f3296468 | 39 | double a = sqrt(3.); |
arayhan878 | 0:1d81f3296468 | 40 | roda1 = 1 * ((-0.5 * x) - (a / 2 * y)) + the; |
arayhan878 | 0:1d81f3296468 | 41 | roda2 = 1 * ((-0.5 * x) + (a / 2 * y)) + the; |
arayhan878 | 0:1d81f3296468 | 42 | roda3 = 1 * (x) + the; |
arayhan878 | 0:1d81f3296468 | 43 | |
arayhan878 | 0:1d81f3296468 | 44 | if (roda1 > 255) |
arayhan878 | 0:1d81f3296468 | 45 | {pwma1 = 255;} |
arayhan878 | 0:1d81f3296468 | 46 | else if (roda1 < -255) |
arayhan878 | 0:1d81f3296468 | 47 | {pwma1 = -255;} |
arayhan878 | 0:1d81f3296468 | 48 | else pwma1 = roda1; |
arayhan878 | 0:1d81f3296468 | 49 | |
arayhan878 | 0:1d81f3296468 | 50 | |
arayhan878 | 0:1d81f3296468 | 51 | if (roda2 > 255) |
arayhan878 | 0:1d81f3296468 | 52 | {pwma2 = 255;} |
arayhan878 | 0:1d81f3296468 | 53 | else if (roda2 < -255) |
arayhan878 | 0:1d81f3296468 | 54 | {pwma2 = -255;} |
arayhan878 | 0:1d81f3296468 | 55 | else pwma2 = roda2; |
arayhan878 | 0:1d81f3296468 | 56 | |
arayhan878 | 0:1d81f3296468 | 57 | if (roda3 > 255) |
arayhan878 | 0:1d81f3296468 | 58 | {pwma3 = 255;} |
arayhan878 | 0:1d81f3296468 | 59 | else if (roda3 < -255) |
arayhan878 | 0:1d81f3296468 | 60 | {pwma3 = -255;} |
arayhan878 | 0:1d81f3296468 | 61 | else pwma3 = roda3; |
arayhan878 | 0:1d81f3296468 | 62 | |
arayhan878 | 0:1d81f3296468 | 63 | if (pwma1 < 0) { |
arayhan878 | 0:1d81f3296468 | 64 | abs_pwm1 = abs(pwma1); |
arayhan878 | 0:1d81f3296468 | 65 | pw1=(float) abs_pwm1/255; |
arayhan878 | 0:1d81f3296468 | 66 | dir1 = 1; |
arayhan878 | 0:1d81f3296468 | 67 | dir2 = 0; |
arayhan878 | 0:1d81f3296468 | 68 | } |
arayhan878 | 0:1d81f3296468 | 69 | else { |
arayhan878 | 0:1d81f3296468 | 70 | abs_pwm1 = pwma1; |
arayhan878 | 0:1d81f3296468 | 71 | pw1=(float) abs_pwm1/255; |
arayhan878 | 0:1d81f3296468 | 72 | dir1 = 0; |
arayhan878 | 0:1d81f3296468 | 73 | dir2 = 1; |
arayhan878 | 0:1d81f3296468 | 74 | } |
arayhan878 | 0:1d81f3296468 | 75 | if (pwma2 < 0) { |
arayhan878 | 0:1d81f3296468 | 76 | abs_pwm2 = abs(pwma2); |
arayhan878 | 0:1d81f3296468 | 77 | pw2=(float) abs_pwm2/255; |
arayhan878 | 0:1d81f3296468 | 78 | dir3 = 1; |
arayhan878 | 0:1d81f3296468 | 79 | dir4 = 0; |
arayhan878 | 0:1d81f3296468 | 80 | } |
arayhan878 | 0:1d81f3296468 | 81 | else { |
arayhan878 | 0:1d81f3296468 | 82 | abs_pwm2 = pwma2; |
arayhan878 | 0:1d81f3296468 | 83 | pw2=(float) abs_pwm2/255; |
arayhan878 | 0:1d81f3296468 | 84 | dir3 = 0; |
arayhan878 | 0:1d81f3296468 | 85 | dir4 = 1; |
arayhan878 | 0:1d81f3296468 | 86 | } |
arayhan878 | 0:1d81f3296468 | 87 | if (pwma3 < 0) { |
arayhan878 | 0:1d81f3296468 | 88 | abs_pwm3 = abs(pwma3); |
arayhan878 | 0:1d81f3296468 | 89 | pw3=(float) abs_pwm3/255; |
arayhan878 | 0:1d81f3296468 | 90 | dir5 = 0; |
arayhan878 | 0:1d81f3296468 | 91 | dir6 = 1; |
arayhan878 | 0:1d81f3296468 | 92 | } |
arayhan878 | 0:1d81f3296468 | 93 | else { |
arayhan878 | 0:1d81f3296468 | 94 | abs_pwm3 = pwma3; |
arayhan878 | 0:1d81f3296468 | 95 | pw3=(float) abs_pwm3/255; |
arayhan878 | 0:1d81f3296468 | 96 | dir5 = 1; |
arayhan878 | 0:1d81f3296468 | 97 | dir6 = 0; |
arayhan878 | 0:1d81f3296468 | 98 | } |
arayhan878 | 0:1d81f3296468 | 99 | } |
arayhan878 | 0:1d81f3296468 | 100 | |
arayhan878 | 0:1d81f3296468 | 101 | void berhenti() { |
arayhan878 | 0:1d81f3296468 | 102 | pwm1 = 0.5; |
arayhan878 | 0:1d81f3296468 | 103 | pwm2 = 0.5; |
arayhan878 | 0:1d81f3296468 | 104 | pwm3 = 0.5; |
arayhan878 | 0:1d81f3296468 | 105 | dir1 = 1; |
arayhan878 | 0:1d81f3296468 | 106 | dir2 = 1; |
arayhan878 | 0:1d81f3296468 | 107 | dir3 = 1; |
arayhan878 | 0:1d81f3296468 | 108 | dir4 = 1; |
arayhan878 | 0:1d81f3296468 | 109 | dir5 = 1; |
arayhan878 | 0:1d81f3296468 | 110 | dir6 = 1; |
arayhan878 | 0:1d81f3296468 | 111 | } |
arayhan878 | 0:1d81f3296468 | 112 | |
arayhan878 | 0:1d81f3296468 | 113 | |
arayhan878 | 0:1d81f3296468 | 114 | int main() |
arayhan878 | 0:1d81f3296468 | 115 | { |
arayhan878 | 0:1d81f3296468 | 116 | pc.baud(9600); |
arayhan878 | 0:1d81f3296468 | 117 | bt.baud(9600); |
arayhan878 | 0:1d81f3296468 | 118 | pc.printf("saaaarttt"); |
arayhan878 | 0:1d81f3296468 | 119 | /////////////////////////////////PID/////////////////////////////// |
arayhan878 | 0:1d81f3296468 | 120 | preError = 0; |
arayhan878 | 0:1d81f3296468 | 121 | integral = 0; |
arayhan878 | 0:1d81f3296468 | 122 | /////////////////////////////////PID///////////////////////////////// |
arayhan878 | 0:1d81f3296468 | 123 | pc.printf("starttttttt\n"); |
arayhan878 | 0:1d81f3296468 | 124 | |
arayhan878 | 0:1d81f3296468 | 125 | |
arayhan878 | 0:1d81f3296468 | 126 | while(1) { |
arayhan878 | 0:1d81f3296468 | 127 | |
arayhan878 | 0:1d81f3296468 | 128 | if(bt.readable()) |
arayhan878 | 0:1d81f3296468 | 129 | { |
arayhan878 | 0:1d81f3296468 | 130 | ch1 = bt.getc(); |
arayhan878 | 0:1d81f3296468 | 131 | ch = ch1 - 90; |
arayhan878 | 0:1d81f3296468 | 132 | if(ch != -34) |
arayhan878 | 0:1d81f3296468 | 133 | { |
arayhan878 | 0:1d81f3296468 | 134 | pc.printf("yapppp "); |
arayhan878 | 0:1d81f3296468 | 135 | pc.printf(" sudut : %d",ch); |
arayhan878 | 0:1d81f3296468 | 136 | |
arayhan878 | 0:1d81f3296468 | 137 | //////////////////////////////////PID/////////////////////////////// |
arayhan878 | 0:1d81f3296468 | 138 | e = setpoint - ch; |
arayhan878 | 0:1d81f3296468 | 139 | integral = integral + (e * Dt); |
arayhan878 | 0:1d81f3296468 | 140 | derivative = (e - preError) / Dt; |
arayhan878 | 0:1d81f3296468 | 141 | output = (Kp * e) + (Ki * integral) + (Kd * derivative); |
arayhan878 | 0:1d81f3296468 | 142 | preError = e; |
arayhan878 | 0:1d81f3296468 | 143 | //////////////////////////////////PID/////////////////////////////// |
arayhan878 | 0:1d81f3296468 | 144 | out = output*-1; |
arayhan878 | 0:1d81f3296468 | 145 | atur(0, 0, out); |
arayhan878 | 0:1d81f3296468 | 146 | //pc.printf(" pw1 : %f",pw1); |
arayhan878 | 0:1d81f3296468 | 147 | //pc.printf(" pw2 : %f",pw2); |
arayhan878 | 0:1d81f3296468 | 148 | //pc.printf(" pw3 : %f",pw3); |
arayhan878 | 0:1d81f3296468 | 149 | |
arayhan878 | 0:1d81f3296468 | 150 | //pc.printf(" I : %d",integral); |
arayhan878 | 0:1d81f3296468 | 151 | //pc.printf(" D : %d",derivative); |
arayhan878 | 0:1d81f3296468 | 152 | //pc.printf(" error : %d",e); |
arayhan878 | 0:1d81f3296468 | 153 | //pc.printf(" pwm : %d",out); |
arayhan878 | 0:1d81f3296468 | 154 | pc.printf(" rod1 = %0.2f ", roda1 ); |
arayhan878 | 0:1d81f3296468 | 155 | pc.printf(" rod2 = %0.2f ", roda2 ); |
arayhan878 | 0:1d81f3296468 | 156 | pc.printf(" rod3 = %0.2f\n ", roda3 ); |
arayhan878 | 0:1d81f3296468 | 157 | pwm1.write(pw1); |
arayhan878 | 0:1d81f3296468 | 158 | pwm2.write(pw2); |
arayhan878 | 0:1d81f3296468 | 159 | pwm3.write(pw3); |
arayhan878 | 0:1d81f3296468 | 160 | } |
arayhan878 | 0:1d81f3296468 | 161 | else |
arayhan878 | 0:1d81f3296468 | 162 | { |
arayhan878 | 0:1d81f3296468 | 163 | pc.printf("nope \n"); |
arayhan878 | 0:1d81f3296468 | 164 | atur(0, 0, 0); |
arayhan878 | 0:1d81f3296468 | 165 | pwm1.write(pw1); |
arayhan878 | 0:1d81f3296468 | 166 | pwm2.write(pw2); |
arayhan878 | 0:1d81f3296468 | 167 | pwm3.write(pw3); |
arayhan878 | 0:1d81f3296468 | 168 | } |
arayhan878 | 0:1d81f3296468 | 169 | } |
arayhan878 | 0:1d81f3296468 | 170 | |
arayhan878 | 0:1d81f3296468 | 171 | |
arayhan878 | 0:1d81f3296468 | 172 | //atur(0, 150, 0); //mundur |
arayhan878 | 0:1d81f3296468 | 173 | //pc.printf("Mundur"); |
arayhan878 | 0:1d81f3296468 | 174 | // pc.printf("Kiri"); |
arayhan878 | 0:1d81f3296468 | 175 | // atur(-150, 0, 0); //kiri |
arayhan878 | 0:1d81f3296468 | 176 | // pwm1.write(pw1); |
arayhan878 | 0:1d81f3296468 | 177 | // pwm2.write(pw2); |
arayhan878 | 0:1d81f3296468 | 178 | // pwm3.write(pw3); |
arayhan878 | 0:1d81f3296468 | 179 | |
arayhan878 | 0:1d81f3296468 | 180 | //} |
arayhan878 | 0:1d81f3296468 | 181 | //else if (data == 'g') { |
arayhan878 | 0:1d81f3296468 | 182 | // pc.printf("Kiri"); |
arayhan878 | 0:1d81f3296468 | 183 | // atur(-60, 0, 0); //kiri |
arayhan878 | 0:1d81f3296468 | 184 | //} |
arayhan878 | 0:1d81f3296468 | 185 | //else if (data == 'i') { |
arayhan878 | 0:1d81f3296468 | 186 | // pc.printf("Kanan"); |
arayhan878 | 0:1d81f3296468 | 187 | // atur(60, 0, 0); //kanan |
arayhan878 | 0:1d81f3296468 | 188 | //} |
arayhan878 | 0:1d81f3296468 | 189 | |
arayhan878 | 0:1d81f3296468 | 190 | //else if (data == 'l') |
arayhan878 | 0:1d81f3296468 | 191 | //{ |
arayhan878 | 0:1d81f3296468 | 192 | // atur(0, 0, -45); //Putar |
arayhan878 | 0:1d81f3296468 | 193 | // pc.printf("Putar Kanan"); |
arayhan878 | 0:1d81f3296468 | 194 | // data = 0; |
arayhan878 | 0:1d81f3296468 | 195 | //} |
arayhan878 | 0:1d81f3296468 | 196 | //else if (data == 'j') |
arayhan878 | 0:1d81f3296468 | 197 | //{ |
arayhan878 | 0:1d81f3296468 | 198 | // atur(0, 0, 45); //Putar |
arayhan878 | 0:1d81f3296468 | 199 | // pc.printf("Putar Kiri"); |
arayhan878 | 0:1d81f3296468 | 200 | // data = 0; |
arayhan878 | 0:1d81f3296468 | 201 | //} |
arayhan878 | 0:1d81f3296468 | 202 | |
arayhan878 | 0:1d81f3296468 | 203 | // else |
arayhan878 | 0:1d81f3296468 | 204 | // { |
arayhan878 | 0:1d81f3296468 | 205 | // berhenti(); |
arayhan878 | 0:1d81f3296468 | 206 | // pc.printf("Stop"); |
arayhan878 | 0:1d81f3296468 | 207 | // } |
arayhan878 | 0:1d81f3296468 | 208 | |
arayhan878 | 0:1d81f3296468 | 209 | } |
arayhan878 | 0:1d81f3296468 | 210 | } |