Yolanda Tania
/
PID_tangan
udah bisa looo
main.cpp@0:aa8e05bc0533, 2020-02-27 (annotated)
- Committer:
- Yolandataniaa
- Date:
- Thu Feb 27 12:40:03 2020 +0000
- Revision:
- 0:aa8e05bc0533
- Child:
- 1:0122c72f6e1b
good pid
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Yolandataniaa | 0:aa8e05bc0533 | 1 | /*************************************************************************** |
Yolandataniaa | 0:aa8e05bc0533 | 2 | * Title : Program Debug Motor dan Encoder |
Yolandataniaa | 0:aa8e05bc0533 | 3 | * Name : debug_motor.cpp |
Yolandataniaa | 0:aa8e05bc0533 | 4 | * Version : 1.0 |
Yolandataniaa | 0:aa8e05bc0533 | 5 | * Author : Gian Arjuna EL 16 |
Yolandataniaa | 0:aa8e05bc0533 | 6 | * Date : 12 Desember 2019 |
Yolandataniaa | 0:aa8e05bc0533 | 7 | * Description: |
Yolandataniaa | 0:aa8e05bc0533 | 8 | * |
Yolandataniaa | 0:aa8e05bc0533 | 9 | * Program ini dapat digunakan untuk mengambil data pulse encoder, menggerakan |
Yolandataniaa | 0:aa8e05bc0533 | 10 | * motor, mengambil data kecepatan, dan mengambil data PID. Untuk menggunakan |
Yolandataniaa | 0:aa8e05bc0533 | 11 | * program ini UBAH DEKLARASI PIN PADA BAGIAN PIN LIST DIBAWAH agar sesuai |
Yolandataniaa | 0:aa8e05bc0533 | 12 | * dengan pin yang sedang digunakan. Kemudian pilih mode debug yang ingin |
Yolandataniaa | 0:aa8e05bc0533 | 13 | * dilakukan dengan comment/uncomment "#define" pada bagian mode debug |
Yolandataniaa | 0:aa8e05bc0533 | 14 | * |
Yolandataniaa | 0:aa8e05bc0533 | 15 | ***************************************************************************/ |
Yolandataniaa | 0:aa8e05bc0533 | 16 | |
Yolandataniaa | 0:aa8e05bc0533 | 17 | |
Yolandataniaa | 0:aa8e05bc0533 | 18 | /* LIBRARY */ |
Yolandataniaa | 0:aa8e05bc0533 | 19 | #include "mbed.h" |
Yolandataniaa | 0:aa8e05bc0533 | 20 | #include "encoderKRAI.h" |
Yolandataniaa | 0:aa8e05bc0533 | 21 | #include "Motor.h" |
Yolandataniaa | 0:aa8e05bc0533 | 22 | #include "pid_dagoz/PID.h" |
Yolandataniaa | 0:aa8e05bc0533 | 23 | |
Yolandataniaa | 0:aa8e05bc0533 | 24 | /* MODE DEBUG YANG DIINGINKAN */ |
Yolandataniaa | 0:aa8e05bc0533 | 25 | //#define AMBIL_ENCODER |
Yolandataniaa | 0:aa8e05bc0533 | 26 | //#define GERAK_MOTOR1 |
Yolandataniaa | 0:aa8e05bc0533 | 27 | //#define GERAK_MOTOR2 |
Yolandataniaa | 0:aa8e05bc0533 | 28 | //#define AMBIL_KECEPATAN |
Yolandataniaa | 0:aa8e05bc0533 | 29 | //#define TES_PID |
Yolandataniaa | 0:aa8e05bc0533 | 30 | #define TES_PID_TANGAN |
Yolandataniaa | 0:aa8e05bc0533 | 31 | |
Yolandataniaa | 0:aa8e05bc0533 | 32 | /***** PIN LIST *****/ |
Yolandataniaa | 0:aa8e05bc0533 | 33 | /* pin assignment untuk encoder */ |
Yolandataniaa | 0:aa8e05bc0533 | 34 | encoderKRAI enc_arm1(PB_7, PB_6, 538, encoderKRAI::X4_ENCODING); |
Yolandataniaa | 0:aa8e05bc0533 | 35 | //encoderKRAI enc_arm2(PB_2, PD_2, 538, encoderKRAI::X4_ENCODING); |
Yolandataniaa | 0:aa8e05bc0533 | 36 | |
Yolandataniaa | 0:aa8e05bc0533 | 37 | /* pin assignment untuk motor */ |
Yolandataniaa | 0:aa8e05bc0533 | 38 | |
Yolandataniaa | 0:aa8e05bc0533 | 39 | Motor arm1(PA_15, PA_13, PA_14); |
Yolandataniaa | 0:aa8e05bc0533 | 40 | //Motor arm2(PC_8, PC_6, PC_5); |
Yolandataniaa | 0:aa8e05bc0533 | 41 | |
Yolandataniaa | 0:aa8e05bc0533 | 42 | float kp1 = 0.0204622,kp2 = 0.0102622; |
Yolandataniaa | 0:aa8e05bc0533 | 43 | float kd1 = 0.06,kd2 = 0.065; |
Yolandataniaa | 0:aa8e05bc0533 | 44 | float ki1 = 0 ,ki2 = 0; |
Yolandataniaa | 0:aa8e05bc0533 | 45 | float ff1 = 0,ff2 = 0; |
Yolandataniaa | 0:aa8e05bc0533 | 46 | float n1 = 0,n2 = 0; |
Yolandataniaa | 0:aa8e05bc0533 | 47 | float ts1 = 0.009127; |
Yolandataniaa | 0:aa8e05bc0533 | 48 | float PI = 3.14159; |
Yolandataniaa | 0:aa8e05bc0533 | 49 | float wheel_radius = 0.075; |
Yolandataniaa | 0:aa8e05bc0533 | 50 | |
Yolandataniaa | 0:aa8e05bc0533 | 51 | PID pid1(kp1, ki1, kd1, n1, ts1, ff1, PID::PI_MODE); |
Yolandataniaa | 0:aa8e05bc0533 | 52 | PID pid2(kp2, ki2, kd2, n2, ts1, ff2, PID::PI_MODE); |
Yolandataniaa | 0:aa8e05bc0533 | 53 | |
Yolandataniaa | 0:aa8e05bc0533 | 54 | /* pin assignment lainnya */ |
Yolandataniaa | 0:aa8e05bc0533 | 55 | DigitalIn mybutton(USER_BUTTON, PullUp); |
Yolandataniaa | 0:aa8e05bc0533 | 56 | |
Yolandataniaa | 0:aa8e05bc0533 | 57 | /* komunikasi serial dengan UART */ |
Yolandataniaa | 0:aa8e05bc0533 | 58 | Serial pc(USBTX, USBRX, 115200); |
Yolandataniaa | 0:aa8e05bc0533 | 59 | |
Yolandataniaa | 0:aa8e05bc0533 | 60 | /* timer untuk mendapatkan waktu */ |
Yolandataniaa | 0:aa8e05bc0533 | 61 | Timer timer1; |
Yolandataniaa | 0:aa8e05bc0533 | 62 | |
Yolandataniaa | 0:aa8e05bc0533 | 63 | |
Yolandataniaa | 0:aa8e05bc0533 | 64 | /* deklarasi variable global */ |
Yolandataniaa | 0:aa8e05bc0533 | 65 | /* array untuk menyimpan data kecepatan */ |
Yolandataniaa | 0:aa8e05bc0533 | 66 | float theta1[400]; |
Yolandataniaa | 0:aa8e05bc0533 | 67 | float theta2[400]; |
Yolandataniaa | 0:aa8e05bc0533 | 68 | float input[400]; |
Yolandataniaa | 0:aa8e05bc0533 | 69 | float time_array[400]; |
Yolandataniaa | 0:aa8e05bc0533 | 70 | |
Yolandataniaa | 0:aa8e05bc0533 | 71 | /* variable kecepatan dan posisi*/ |
Yolandataniaa | 0:aa8e05bc0533 | 72 | float curr_theta, prev_theta; |
Yolandataniaa | 0:aa8e05bc0533 | 73 | float en1,en2; |
Yolandataniaa | 0:aa8e05bc0533 | 74 | |
Yolandataniaa | 0:aa8e05bc0533 | 75 | /* variable sampling time */ |
Yolandataniaa | 0:aa8e05bc0533 | 76 | int samp, samp_pid, TS; |
Yolandataniaa | 0:aa8e05bc0533 | 77 | int i; |
Yolandataniaa | 0:aa8e05bc0533 | 78 | int counttt; |
Yolandataniaa | 0:aa8e05bc0533 | 79 | |
Yolandataniaa | 0:aa8e05bc0533 | 80 | |
Yolandataniaa | 0:aa8e05bc0533 | 81 | /* variable pid */ |
Yolandataniaa | 0:aa8e05bc0533 | 82 | float curr_theta1,curr_theta2; |
Yolandataniaa | 0:aa8e05bc0533 | 83 | float prev_error, lowpass_error, prev_lowpass_error; |
Yolandataniaa | 0:aa8e05bc0533 | 84 | float pwm1,pwm2; |
Yolandataniaa | 0:aa8e05bc0533 | 85 | |
Yolandataniaa | 0:aa8e05bc0533 | 86 | float teta_ref; |
Yolandataniaa | 0:aa8e05bc0533 | 87 | float teta_act; |
Yolandataniaa | 0:aa8e05bc0533 | 88 | float kp_teta = 0.005; |
Yolandataniaa | 0:aa8e05bc0533 | 89 | float ki_teta = 0; |
Yolandataniaa | 0:aa8e05bc0533 | 90 | float kd_teta = 0; |
Yolandataniaa | 0:aa8e05bc0533 | 91 | float TS_pid; |
Yolandataniaa | 0:aa8e05bc0533 | 92 | float w_ref; |
Yolandataniaa | 0:aa8e05bc0533 | 93 | float prev_teta_ref; |
Yolandataniaa | 0:aa8e05bc0533 | 94 | float w_act; |
Yolandataniaa | 0:aa8e05bc0533 | 95 | float kp_w = 0.005; |
Yolandataniaa | 0:aa8e05bc0533 | 96 | float ki_w = 0; |
Yolandataniaa | 0:aa8e05bc0533 | 97 | float kd_w = 0.005; |
Yolandataniaa | 0:aa8e05bc0533 | 98 | float pwm; |
Yolandataniaa | 0:aa8e05bc0533 | 99 | |
Yolandataniaa | 0:aa8e05bc0533 | 100 | /* variable penyimpan waktu */ |
Yolandataniaa | 0:aa8e05bc0533 | 101 | uint32_t last_baca, last_pid, last_motor; |
Yolandataniaa | 0:aa8e05bc0533 | 102 | |
Yolandataniaa | 0:aa8e05bc0533 | 103 | /* prototipe fungsi */ |
Yolandataniaa | 0:aa8e05bc0533 | 104 | //void pid (float ref, float curr_feed, float prev_feed, float feedforward, float actual, float kp, float ki, float kd, float TS, float* output); |
Yolandataniaa | 0:aa8e05bc0533 | 105 | |
Yolandataniaa | 0:aa8e05bc0533 | 106 | int main() { |
Yolandataniaa | 0:aa8e05bc0533 | 107 | arm1.period(0.02); |
Yolandataniaa | 0:aa8e05bc0533 | 108 | /* ================================================================== */ |
Yolandataniaa | 0:aa8e05bc0533 | 109 | #ifdef AMBIL_ENCODER |
Yolandataniaa | 0:aa8e05bc0533 | 110 | while(1) |
Yolandataniaa | 0:aa8e05bc0533 | 111 | { |
Yolandataniaa | 0:aa8e05bc0533 | 112 | en1= (float)enc_arm1.getPulses(); |
Yolandataniaa | 0:aa8e05bc0533 | 113 | //en2= (float)enc_arm2.getPulses(); |
Yolandataniaa | 0:aa8e05bc0533 | 114 | pc.printf("%f\t %f\n", en1, en2); |
Yolandataniaa | 0:aa8e05bc0533 | 115 | } |
Yolandataniaa | 0:aa8e05bc0533 | 116 | #endif |
Yolandataniaa | 0:aa8e05bc0533 | 117 | |
Yolandataniaa | 0:aa8e05bc0533 | 118 | /* ================================================================== */ |
Yolandataniaa | 0:aa8e05bc0533 | 119 | |
Yolandataniaa | 0:aa8e05bc0533 | 120 | #ifdef GERAK_MOTOR1 |
Yolandataniaa | 0:aa8e05bc0533 | 121 | while(1) |
Yolandataniaa | 0:aa8e05bc0533 | 122 | { |
Yolandataniaa | 0:aa8e05bc0533 | 123 | float pwm = 0.6; |
Yolandataniaa | 0:aa8e05bc0533 | 124 | arm1.speed(pwm); |
Yolandataniaa | 0:aa8e05bc0533 | 125 | en1= (float)enc_arm1.getPulses(); |
Yolandataniaa | 0:aa8e05bc0533 | 126 | pc.printf("%f\t %f\n", en1, en2); |
Yolandataniaa | 0:aa8e05bc0533 | 127 | } |
Yolandataniaa | 0:aa8e05bc0533 | 128 | #endif |
Yolandataniaa | 0:aa8e05bc0533 | 129 | |
Yolandataniaa | 0:aa8e05bc0533 | 130 | #ifdef GERAK_MOTOR2 |
Yolandataniaa | 0:aa8e05bc0533 | 131 | while(1) |
Yolandataniaa | 0:aa8e05bc0533 | 132 | { |
Yolandataniaa | 0:aa8e05bc0533 | 133 | float pwm = 0.6; |
Yolandataniaa | 0:aa8e05bc0533 | 134 | arm2.speed(pwm); |
Yolandataniaa | 0:aa8e05bc0533 | 135 | //en2= (float)enc_arm2.getPulses(); |
Yolandataniaa | 0:aa8e05bc0533 | 136 | pc.printf("%f\t %f\n", en1, en2); |
Yolandataniaa | 0:aa8e05bc0533 | 137 | } |
Yolandataniaa | 0:aa8e05bc0533 | 138 | #endif |
Yolandataniaa | 0:aa8e05bc0533 | 139 | /* ================================================================== */ |
Yolandataniaa | 0:aa8e05bc0533 | 140 | #ifdef AMBIL_KECEPATAN // (misal arm1(?)) |
Yolandataniaa | 0:aa8e05bc0533 | 141 | |
Yolandataniaa | 0:aa8e05bc0533 | 142 | /* setup and initialization*/ |
Yolandataniaa | 0:aa8e05bc0533 | 143 | timer1.start(); |
Yolandataniaa | 0:aa8e05bc0533 | 144 | |
Yolandataniaa | 0:aa8e05bc0533 | 145 | /* command move motor and sample data*/ |
Yolandataniaa | 0:aa8e05bc0533 | 146 | while(counttt <= 400) |
Yolandataniaa | 0:aa8e05bc0533 | 147 | { |
Yolandataniaa | 0:aa8e05bc0533 | 148 | if (t.read_us()-samp >= 5000) |
Yolandataniaa | 0:aa8e05bc0533 | 149 | { |
Yolandataniaa | 0:aa8e05bc0533 | 150 | TS = t.read_us()-samp; |
Yolandataniaa | 0:aa8e05bc0533 | 151 | curr_theta = (float)enc.getPulses()*360/538; |
Yolandataniaa | 0:aa8e05bc0533 | 152 | theta1[counttt] = curr_theta1; |
Yolandataniaa | 0:aa8e05bc0533 | 153 | enc_arm1.reset(); |
Yolandataniaa | 0:aa8e05bc0533 | 154 | arm1.speed(0.3); |
Yolandataniaa | 0:aa8e05bc0533 | 155 | counttt ++; |
Yolandataniaa | 0:aa8e05bc0533 | 156 | samp = t.read_us(); |
Yolandataniaa | 0:aa8e05bc0533 | 157 | } |
Yolandataniaa | 0:aa8e05bc0533 | 158 | } |
Yolandataniaa | 0:aa8e05bc0533 | 159 | arm1.speed(0); /* turn off motor after sampling done */ |
Yolandataniaa | 0:aa8e05bc0533 | 160 | |
Yolandataniaa | 0:aa8e05bc0533 | 161 | /* print data */ |
Yolandataniaa | 0:aa8e05bc0533 | 162 | for(i = 0; i < 400; i++) |
Yolandataniaa | 0:aa8e05bc0533 | 163 | { |
Yolandataniaa | 0:aa8e05bc0533 | 164 | pc.printf("%f\n", theta[i]); |
Yolandataniaa | 0:aa8e05bc0533 | 165 | } |
Yolandataniaa | 0:aa8e05bc0533 | 166 | #endif |
Yolandataniaa | 0:aa8e05bc0533 | 167 | /* ================================================================== */ |
Yolandataniaa | 0:aa8e05bc0533 | 168 | |
Yolandataniaa | 0:aa8e05bc0533 | 169 | #ifdef TES_PID |
Yolandataniaa | 0:aa8e05bc0533 | 170 | /* setup and initialization*/ |
Yolandataniaa | 0:aa8e05bc0533 | 171 | timer1.start(); |
Yolandataniaa | 0:aa8e05bc0533 | 172 | float period = 0.0005; |
Yolandataniaa | 0:aa8e05bc0533 | 173 | arm1.period(period); |
Yolandataniaa | 0:aa8e05bc0533 | 174 | //arm2.period(period); |
Yolandataniaa | 0:aa8e05bc0533 | 175 | // float pwmz = 0.7; |
Yolandataniaa | 0:aa8e05bc0533 | 176 | |
Yolandataniaa | 0:aa8e05bc0533 | 177 | /* command move motor and sample data*/ |
Yolandataniaa | 0:aa8e05bc0533 | 178 | while(counttt <= 400) |
Yolandataniaa | 0:aa8e05bc0533 | 179 | { |
Yolandataniaa | 0:aa8e05bc0533 | 180 | if (timer1.read_us()-samp >= 7123) |
Yolandataniaa | 0:aa8e05bc0533 | 181 | { |
Yolandataniaa | 0:aa8e05bc0533 | 182 | TS = timer1.read_us()-samp; |
Yolandataniaa | 0:aa8e05bc0533 | 183 | time_array[counttt] = (float)timer1.read_us()/1000000; |
Yolandataniaa | 0:aa8e05bc0533 | 184 | input[counttt] = pwmz*24; |
Yolandataniaa | 0:aa8e05bc0533 | 185 | curr_theta1 = (float)enc_arm1.getPulses()*360/538; |
Yolandataniaa | 0:aa8e05bc0533 | 186 | //curr_theta2 = (float)enc_arm2.getPulses()*360/538; |
Yolandataniaa | 0:aa8e05bc0533 | 187 | theta1[counttt] = curr_theta1; |
Yolandataniaa | 0:aa8e05bc0533 | 188 | //theta2[counttt] = curr_theta2; |
Yolandataniaa | 0:aa8e05bc0533 | 189 | enc_arm1.reset(); |
Yolandataniaa | 0:aa8e05bc0533 | 190 | //enc_arm2.reset(); |
Yolandataniaa | 0:aa8e05bc0533 | 191 | |
Yolandataniaa | 0:aa8e05bc0533 | 192 | counttt ++; |
Yolandataniaa | 0:aa8e05bc0533 | 193 | samp = timer1.read_us(); |
Yolandataniaa | 0:aa8e05bc0533 | 194 | } |
Yolandataniaa | 0:aa8e05bc0533 | 195 | if (timer1.read_us() - samp_pid > 9127) |
Yolandataniaa | 0:aa8e05bc0533 | 196 | { |
Yolandataniaa | 0:aa8e05bc0533 | 197 | float setpoint = 180; |
Yolandataniaa | 0:aa8e05bc0533 | 198 | pwm1 = pid1.createpwm(setpoint,curr_theta1); |
Yolandataniaa | 0:aa8e05bc0533 | 199 | //pwm2 = pid2.createpwm(setpoint,curr_theta2); |
Yolandataniaa | 0:aa8e05bc0533 | 200 | |
Yolandataniaa | 0:aa8e05bc0533 | 201 | |
Yolandataniaa | 0:aa8e05bc0533 | 202 | arm1.speed(pwm1); |
Yolandataniaa | 0:aa8e05bc0533 | 203 | //arm2.speed(pwm2); |
Yolandataniaa | 0:aa8e05bc0533 | 204 | if (curr_theta1>180){ |
Yolandataniaa | 0:aa8e05bc0533 | 205 | arm1.speed(0); |
Yolandataniaa | 0:aa8e05bc0533 | 206 | } |
Yolandataniaa | 0:aa8e05bc0533 | 207 | samp_pid = timer1.read_us(); |
Yolandataniaa | 0:aa8e05bc0533 | 208 | } |
Yolandataniaa | 0:aa8e05bc0533 | 209 | |
Yolandataniaa | 0:aa8e05bc0533 | 210 | } |
Yolandataniaa | 0:aa8e05bc0533 | 211 | |
Yolandataniaa | 0:aa8e05bc0533 | 212 | |
Yolandataniaa | 0:aa8e05bc0533 | 213 | arm1.speed(0); |
Yolandataniaa | 0:aa8e05bc0533 | 214 | arm2.speed(0); |
Yolandataniaa | 0:aa8e05bc0533 | 215 | /* turn off motor after sampling done */ |
Yolandataniaa | 0:aa8e05bc0533 | 216 | |
Yolandataniaa | 0:aa8e05bc0533 | 217 | /* print data */ |
Yolandataniaa | 0:aa8e05bc0533 | 218 | for(i = 0; i < 400; i++) |
Yolandataniaa | 0:aa8e05bc0533 | 219 | { |
Yolandataniaa | 0:aa8e05bc0533 | 220 | pc.printf("%f %f %f %f \n", theta1[i], theta2[i], time_array[i], input[i]); |
Yolandataniaa | 0:aa8e05bc0533 | 221 | } |
Yolandataniaa | 0:aa8e05bc0533 | 222 | #endif |
Yolandataniaa | 0:aa8e05bc0533 | 223 | |
Yolandataniaa | 0:aa8e05bc0533 | 224 | #ifdef TES_PID_TANGAN |
Yolandataniaa | 0:aa8e05bc0533 | 225 | /* setup and initialization*/ |
Yolandataniaa | 0:aa8e05bc0533 | 226 | timer1.start(); |
Yolandataniaa | 0:aa8e05bc0533 | 227 | float period = 0.0005; |
Yolandataniaa | 0:aa8e05bc0533 | 228 | arm1.period(period); |
Yolandataniaa | 0:aa8e05bc0533 | 229 | |
Yolandataniaa | 0:aa8e05bc0533 | 230 | /* command move motor and sample data*/ |
Yolandataniaa | 0:aa8e05bc0533 | 231 | while(1) |
Yolandataniaa | 0:aa8e05bc0533 | 232 | { |
Yolandataniaa | 0:aa8e05bc0533 | 233 | if (timer1.read_us()-samp >= 7123) |
Yolandataniaa | 0:aa8e05bc0533 | 234 | { |
Yolandataniaa | 0:aa8e05bc0533 | 235 | curr_theta1 += (float)enc_arm1.getPulses()*360/538; |
Yolandataniaa | 0:aa8e05bc0533 | 236 | //curr_theta2 += (float)enc_arm2.getPulses()*360/538; |
Yolandataniaa | 0:aa8e05bc0533 | 237 | enc_arm1.reset(); |
Yolandataniaa | 0:aa8e05bc0533 | 238 | //enc_arm2.reset(); |
Yolandataniaa | 0:aa8e05bc0533 | 239 | |
Yolandataniaa | 0:aa8e05bc0533 | 240 | samp = timer1.read_us(); |
Yolandataniaa | 0:aa8e05bc0533 | 241 | } |
Yolandataniaa | 0:aa8e05bc0533 | 242 | if (timer1.read_us() - samp_pid > 9127) |
Yolandataniaa | 0:aa8e05bc0533 | 243 | { |
Yolandataniaa | 0:aa8e05bc0533 | 244 | // float setpoint = 110; |
Yolandataniaa | 0:aa8e05bc0533 | 245 | float setpoint = 150; |
Yolandataniaa | 0:aa8e05bc0533 | 246 | float error = setpoint - curr_theta1; |
Yolandataniaa | 0:aa8e05bc0533 | 247 | |
Yolandataniaa | 0:aa8e05bc0533 | 248 | lowpass_error = 0.1*error + 0.9*prev_error; |
Yolandataniaa | 0:aa8e05bc0533 | 249 | // lowpass_error = 0.1*lowpass_error + 0.9*prev_lowpass_error; |
Yolandataniaa | 0:aa8e05bc0533 | 250 | // prev_lowpass_error= lowpass_error; |
Yolandataniaa | 0:aa8e05bc0533 | 251 | |
Yolandataniaa | 0:aa8e05bc0533 | 252 | pwm1 = kp2*(lowpass_error) + kd2*(lowpass_error - prev_lowpass_error); |
Yolandataniaa | 0:aa8e05bc0533 | 253 | pwm1 = fabs(pwm1) > 0.85 ? 0.85*fabs(pwm1)/pwm1 : pwm1; |
Yolandataniaa | 0:aa8e05bc0533 | 254 | |
Yolandataniaa | 0:aa8e05bc0533 | 255 | prev_lowpass_error = lowpass_error; |
Yolandataniaa | 0:aa8e05bc0533 | 256 | |
Yolandataniaa | 0:aa8e05bc0533 | 257 | prev_error = error; |
Yolandataniaa | 0:aa8e05bc0533 | 258 | |
Yolandataniaa | 0:aa8e05bc0533 | 259 | //arm2.speed(pwm2); |
Yolandataniaa | 0:aa8e05bc0533 | 260 | // if (curr_theta1>110 && curr_theta1 < 140 && setpoint >= 120){ |
Yolandataniaa | 0:aa8e05bc0533 | 261 | if (curr_theta1>140 && curr_theta1 < 170 && setpoint >= 145){ |
Yolandataniaa | 0:aa8e05bc0533 | 262 | arm1.speed(0); |
Yolandataniaa | 0:aa8e05bc0533 | 263 | arm1.brake(); |
Yolandataniaa | 0:aa8e05bc0533 | 264 | } |
Yolandataniaa | 0:aa8e05bc0533 | 265 | else if (curr_theta1>130 && curr_theta1 <= 140){ |
Yolandataniaa | 0:aa8e05bc0533 | 266 | arm1.speed(0.075*pwm1); |
Yolandataniaa | 0:aa8e05bc0533 | 267 | } |
Yolandataniaa | 0:aa8e05bc0533 | 268 | else if (curr_theta1>115 && curr_theta1 <= 130){ |
Yolandataniaa | 0:aa8e05bc0533 | 269 | arm1.speed(0.65*pwm1); |
Yolandataniaa | 0:aa8e05bc0533 | 270 | } |
Yolandataniaa | 0:aa8e05bc0533 | 271 | else { |
Yolandataniaa | 0:aa8e05bc0533 | 272 | arm1.speed(pwm1); |
Yolandataniaa | 0:aa8e05bc0533 | 273 | } |
Yolandataniaa | 0:aa8e05bc0533 | 274 | samp_pid = timer1.read_us(); |
Yolandataniaa | 0:aa8e05bc0533 | 275 | |
Yolandataniaa | 0:aa8e05bc0533 | 276 | |
Yolandataniaa | 0:aa8e05bc0533 | 277 | } |
Yolandataniaa | 0:aa8e05bc0533 | 278 | if(timer1.read_us() - last_baca > 100000){ |
Yolandataniaa | 0:aa8e05bc0533 | 279 | pc.printf("%.2f %.2f\n", curr_theta1, pwm1); |
Yolandataniaa | 0:aa8e05bc0533 | 280 | } |
Yolandataniaa | 0:aa8e05bc0533 | 281 | |
Yolandataniaa | 0:aa8e05bc0533 | 282 | } |
Yolandataniaa | 0:aa8e05bc0533 | 283 | /* turn off motor after sampling done */ |
Yolandataniaa | 0:aa8e05bc0533 | 284 | #endif |
Yolandataniaa | 0:aa8e05bc0533 | 285 | /* ================================================================== */ |
Yolandataniaa | 0:aa8e05bc0533 | 286 | |
Yolandataniaa | 0:aa8e05bc0533 | 287 | } |
Yolandataniaa | 0:aa8e05bc0533 | 288 | |
Yolandataniaa | 0:aa8e05bc0533 | 289 | |
Yolandataniaa | 0:aa8e05bc0533 | 290 | /* definisi fungsi */ |