KATAPANG / Mbed 2 deprecated PID

bla

Dependencies:   mbed SRF05

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?

UserRevisionLine numberNew 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 }