Yolanda Tania / Mbed 2 deprecated PID_tangan

udah bisa looo

Dependencies:   mbed

main.cpp@1:0122c72f6e1b, 2020-02-27 (annotated)

Committer:
Yolandataniaa
Date:
Thu Feb 27 13:10:57 2020 +0000
Revision:
1:0122c72f6e1b
Parent:
0:aa8e05bc0533 
tangan kanan kamis 27 feb

Who changed what in which revision?

UserRevisionLine numberNew 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 1:0122c72f6e1b 105 void pidyol(float target,float feedback);
Yolandataniaa 0:aa8e05bc0533 106
Yolandataniaa 0:aa8e05bc0533 107 int main() {
Yolandataniaa 0:aa8e05bc0533 108 arm1.period(0.02);
Yolandataniaa 0:aa8e05bc0533 109 /* ================================================================== */
Yolandataniaa 0:aa8e05bc0533 110 #ifdef AMBIL_ENCODER
Yolandataniaa 0:aa8e05bc0533 111 while(1)
Yolandataniaa 0:aa8e05bc0533 112 {
Yolandataniaa 0:aa8e05bc0533 113 en1= (float)enc_arm1.getPulses();
Yolandataniaa 0:aa8e05bc0533 114 //en2= (float)enc_arm2.getPulses();
Yolandataniaa 0:aa8e05bc0533 115 pc.printf("%f\t %f\n", en1, en2);
Yolandataniaa 0:aa8e05bc0533 116 }
Yolandataniaa 0:aa8e05bc0533 117 #endif
Yolandataniaa 0:aa8e05bc0533 118
Yolandataniaa 0:aa8e05bc0533 119 /* ================================================================== */
Yolandataniaa 0:aa8e05bc0533 120
Yolandataniaa 0:aa8e05bc0533 121 #ifdef GERAK_MOTOR1
Yolandataniaa 0:aa8e05bc0533 122 while(1)
Yolandataniaa 0:aa8e05bc0533 123 {
Yolandataniaa 0:aa8e05bc0533 124 float pwm = 0.6;
Yolandataniaa 0:aa8e05bc0533 125 arm1.speed(pwm);
Yolandataniaa 0:aa8e05bc0533 126 en1= (float)enc_arm1.getPulses();
Yolandataniaa 0:aa8e05bc0533 127 pc.printf("%f\t %f\n", en1, en2);
Yolandataniaa 0:aa8e05bc0533 128 }
Yolandataniaa 0:aa8e05bc0533 129 #endif
Yolandataniaa 0:aa8e05bc0533 130
Yolandataniaa 0:aa8e05bc0533 131 #ifdef GERAK_MOTOR2
Yolandataniaa 0:aa8e05bc0533 132 while(1)
Yolandataniaa 0:aa8e05bc0533 133 {
Yolandataniaa 0:aa8e05bc0533 134 float pwm = 0.6;
Yolandataniaa 0:aa8e05bc0533 135 arm2.speed(pwm);
Yolandataniaa 0:aa8e05bc0533 136 //en2= (float)enc_arm2.getPulses();
Yolandataniaa 0:aa8e05bc0533 137 pc.printf("%f\t %f\n", en1, en2);
Yolandataniaa 0:aa8e05bc0533 138 }
Yolandataniaa 0:aa8e05bc0533 139 #endif
Yolandataniaa 0:aa8e05bc0533 140 /* ================================================================== */
Yolandataniaa 0:aa8e05bc0533 141 #ifdef AMBIL_KECEPATAN // (misal arm1(?))
Yolandataniaa 0:aa8e05bc0533 142
Yolandataniaa 0:aa8e05bc0533 143 /* setup and initialization*/
Yolandataniaa 0:aa8e05bc0533 144 timer1.start();
Yolandataniaa 0:aa8e05bc0533 145
Yolandataniaa 0:aa8e05bc0533 146 /* command move motor and sample data*/
Yolandataniaa 0:aa8e05bc0533 147 while(counttt <= 400)
Yolandataniaa 0:aa8e05bc0533 148 {
Yolandataniaa 0:aa8e05bc0533 149 if (t.read_us()-samp >= 5000)
Yolandataniaa 0:aa8e05bc0533 150 {
Yolandataniaa 0:aa8e05bc0533 151 TS = t.read_us()-samp;
Yolandataniaa 0:aa8e05bc0533 152 curr_theta = (float)enc.getPulses()*360/538;
Yolandataniaa 0:aa8e05bc0533 153 theta1[counttt] = curr_theta1;
Yolandataniaa 0:aa8e05bc0533 154 enc_arm1.reset();
Yolandataniaa 0:aa8e05bc0533 155 arm1.speed(0.3);
Yolandataniaa 0:aa8e05bc0533 156 counttt ++;
Yolandataniaa 0:aa8e05bc0533 157 samp = t.read_us();
Yolandataniaa 0:aa8e05bc0533 158 }
Yolandataniaa 0:aa8e05bc0533 159 }
Yolandataniaa 0:aa8e05bc0533 160 arm1.speed(0); /* turn off motor after sampling done */
Yolandataniaa 0:aa8e05bc0533 161
Yolandataniaa 0:aa8e05bc0533 162 /* print data */
Yolandataniaa 0:aa8e05bc0533 163 for(i = 0; i < 400; i++)
Yolandataniaa 0:aa8e05bc0533 164 {
Yolandataniaa 0:aa8e05bc0533 165 pc.printf("%f\n", theta[i]);
Yolandataniaa 0:aa8e05bc0533 166 }
Yolandataniaa 0:aa8e05bc0533 167 #endif
Yolandataniaa 0:aa8e05bc0533 168 /* ================================================================== */
Yolandataniaa 0:aa8e05bc0533 169
Yolandataniaa 0:aa8e05bc0533 170 #ifdef TES_PID
Yolandataniaa 0:aa8e05bc0533 171 /* setup and initialization*/
Yolandataniaa 0:aa8e05bc0533 172 timer1.start();
Yolandataniaa 0:aa8e05bc0533 173 float period = 0.0005;
Yolandataniaa 0:aa8e05bc0533 174 arm1.period(period);
Yolandataniaa 0:aa8e05bc0533 175 //arm2.period(period);
Yolandataniaa 0:aa8e05bc0533 176 // float pwmz = 0.7;
Yolandataniaa 0:aa8e05bc0533 177
Yolandataniaa 0:aa8e05bc0533 178 /* command move motor and sample data*/
Yolandataniaa 0:aa8e05bc0533 179 while(counttt <= 400)
Yolandataniaa 0:aa8e05bc0533 180 {
Yolandataniaa 0:aa8e05bc0533 181 if (timer1.read_us()-samp >= 7123)
Yolandataniaa 0:aa8e05bc0533 182 {
Yolandataniaa 0:aa8e05bc0533 183 TS = timer1.read_us()-samp;
Yolandataniaa 0:aa8e05bc0533 184 time_array[counttt] = (float)timer1.read_us()/1000000;
Yolandataniaa 0:aa8e05bc0533 185 input[counttt] = pwmz*24;
Yolandataniaa 0:aa8e05bc0533 186 curr_theta1 = (float)enc_arm1.getPulses()*360/538;
Yolandataniaa 0:aa8e05bc0533 187 //curr_theta2 = (float)enc_arm2.getPulses()*360/538;
Yolandataniaa 0:aa8e05bc0533 188 theta1[counttt] = curr_theta1;
Yolandataniaa 0:aa8e05bc0533 189 //theta2[counttt] = curr_theta2;
Yolandataniaa 0:aa8e05bc0533 190 enc_arm1.reset();
Yolandataniaa 0:aa8e05bc0533 191 //enc_arm2.reset();
Yolandataniaa 0:aa8e05bc0533 192
Yolandataniaa 0:aa8e05bc0533 193 counttt ++;
Yolandataniaa 0:aa8e05bc0533 194 samp = timer1.read_us();
Yolandataniaa 0:aa8e05bc0533 195 }
Yolandataniaa 0:aa8e05bc0533 196 if (timer1.read_us() - samp_pid > 9127)
Yolandataniaa 0:aa8e05bc0533 197 {
Yolandataniaa 0:aa8e05bc0533 198 float setpoint = 180;
Yolandataniaa 0:aa8e05bc0533 199 pwm1 = pid1.createpwm(setpoint,curr_theta1);
Yolandataniaa 0:aa8e05bc0533 200 //pwm2 = pid2.createpwm(setpoint,curr_theta2);
Yolandataniaa 0:aa8e05bc0533 201
Yolandataniaa 0:aa8e05bc0533 202
Yolandataniaa 0:aa8e05bc0533 203 arm1.speed(pwm1);
Yolandataniaa 0:aa8e05bc0533 204 //arm2.speed(pwm2);
Yolandataniaa 0:aa8e05bc0533 205 if (curr_theta1>180){
Yolandataniaa 0:aa8e05bc0533 206 arm1.speed(0);
Yolandataniaa 0:aa8e05bc0533 207 }
Yolandataniaa 0:aa8e05bc0533 208 samp_pid = timer1.read_us();
Yolandataniaa 0:aa8e05bc0533 209 }
Yolandataniaa 0:aa8e05bc0533 210
Yolandataniaa 0:aa8e05bc0533 211 }
Yolandataniaa 0:aa8e05bc0533 212
Yolandataniaa 0:aa8e05bc0533 213
Yolandataniaa 0:aa8e05bc0533 214 arm1.speed(0);
Yolandataniaa 0:aa8e05bc0533 215 arm2.speed(0);
Yolandataniaa 0:aa8e05bc0533 216 /* turn off motor after sampling done */
Yolandataniaa 0:aa8e05bc0533 217
Yolandataniaa 0:aa8e05bc0533 218 /* print data */
Yolandataniaa 0:aa8e05bc0533 219 for(i = 0; i < 400; i++)
Yolandataniaa 0:aa8e05bc0533 220 {
Yolandataniaa 0:aa8e05bc0533 221 pc.printf("%f %f %f %f \n", theta1[i], theta2[i], time_array[i], input[i]);
Yolandataniaa 0:aa8e05bc0533 222 }
Yolandataniaa 0:aa8e05bc0533 223 #endif
Yolandataniaa 0:aa8e05bc0533 224
Yolandataniaa 0:aa8e05bc0533 225 #ifdef TES_PID_TANGAN
Yolandataniaa 0:aa8e05bc0533 226 /* setup and initialization*/
Yolandataniaa 0:aa8e05bc0533 227 timer1.start();
Yolandataniaa 0:aa8e05bc0533 228 float period = 0.0005;
Yolandataniaa 1:0122c72f6e1b 229 float setpoint = 150;
Yolandataniaa 0:aa8e05bc0533 230 arm1.period(period);
Yolandataniaa 0:aa8e05bc0533 231
Yolandataniaa 0:aa8e05bc0533 232 /* command move motor and sample data*/
Yolandataniaa 0:aa8e05bc0533 233 while(1)
Yolandataniaa 0:aa8e05bc0533 234 {
Yolandataniaa 0:aa8e05bc0533 235 if (timer1.read_us()-samp >= 7123)
Yolandataniaa 0:aa8e05bc0533 236 {
Yolandataniaa 0:aa8e05bc0533 237 curr_theta1 += (float)enc_arm1.getPulses()*360/538;
Yolandataniaa 0:aa8e05bc0533 238 //curr_theta2 += (float)enc_arm2.getPulses()*360/538;
Yolandataniaa 0:aa8e05bc0533 239 enc_arm1.reset();
Yolandataniaa 0:aa8e05bc0533 240 //enc_arm2.reset();
Yolandataniaa 0:aa8e05bc0533 241
Yolandataniaa 0:aa8e05bc0533 242 samp = timer1.read_us();
Yolandataniaa 0:aa8e05bc0533 243 }
Yolandataniaa 1:0122c72f6e1b 244 if(!mybutton){
Yolandataniaa 1:0122c72f6e1b 245 setpoint = 0;
Yolandataniaa 1:0122c72f6e1b 246 }
Yolandataniaa 0:aa8e05bc0533 247 if (timer1.read_us() - samp_pid > 9127)
Yolandataniaa 0:aa8e05bc0533 248 {
Yolandataniaa 1:0122c72f6e1b 249 pidyol(setpoint,curr_theta1);
Yolandataniaa 0:aa8e05bc0533 250 samp_pid = timer1.read_us();
Yolandataniaa 0:aa8e05bc0533 251
Yolandataniaa 0:aa8e05bc0533 252
Yolandataniaa 0:aa8e05bc0533 253 }
Yolandataniaa 0:aa8e05bc0533 254 if(timer1.read_us() - last_baca > 100000){
Yolandataniaa 0:aa8e05bc0533 255 pc.printf("%.2f %.2f\n", curr_theta1, pwm1);
Yolandataniaa 0:aa8e05bc0533 256 }
Yolandataniaa 0:aa8e05bc0533 257
Yolandataniaa 0:aa8e05bc0533 258 }
Yolandataniaa 0:aa8e05bc0533 259 /* turn off motor after sampling done */
Yolandataniaa 0:aa8e05bc0533 260 #endif
Yolandataniaa 0:aa8e05bc0533 261 /* ================================================================== */
Yolandataniaa 0:aa8e05bc0533 262
Yolandataniaa 0:aa8e05bc0533 263 }
Yolandataniaa 0:aa8e05bc0533 264
Yolandataniaa 0:aa8e05bc0533 265
Yolandataniaa 0:aa8e05bc0533 266 /* definisi fungsi */
Yolandataniaa 1:0122c72f6e1b 267 void pidyol(float target,float feedback){
Yolandataniaa 1:0122c72f6e1b 268 float error = target - feedback;
Yolandataniaa 1:0122c72f6e1b 269
Yolandataniaa 1:0122c72f6e1b 270 lowpass_error = 0.1*error + 0.9*prev_error;
Yolandataniaa 1:0122c72f6e1b 271 // lowpass_error = 0.1*lowpass_error + 0.9*prev_lowpass_error;
Yolandataniaa 1:0122c72f6e1b 272 // prev_lowpass_error= lowpass_error;
Yolandataniaa 1:0122c72f6e1b 273
Yolandataniaa 1:0122c72f6e1b 274 pwm1 = kp2*(lowpass_error) + kd2*(lowpass_error - prev_lowpass_error);
Yolandataniaa 1:0122c72f6e1b 275 pwm1 = fabs(pwm1) > 0.85 ? 0.85*fabs(pwm1)/pwm1 : pwm1;
Yolandataniaa 1:0122c72f6e1b 276
Yolandataniaa 1:0122c72f6e1b 277 prev_lowpass_error = lowpass_error;
Yolandataniaa 1:0122c72f6e1b 278
Yolandataniaa 1:0122c72f6e1b 279 prev_error = error;
Yolandataniaa 1:0122c72f6e1b 280
Yolandataniaa 1:0122c72f6e1b 281 //arm2.speed(pwm2);
Yolandataniaa 1:0122c72f6e1b 282 // if (curr_theta1>110 && curr_theta1 < 140 && setpoint >= 120){
Yolandataniaa 1:0122c72f6e1b 283 if (error>0){
Yolandataniaa 1:0122c72f6e1b 284 if (feedback>140 && feedback < 170 && target >= 145){
Yolandataniaa 1:0122c72f6e1b 285 arm1.speed(0);
Yolandataniaa 1:0122c72f6e1b 286 arm1.brake();
Yolandataniaa 1:0122c72f6e1b 287 }
Yolandataniaa 1:0122c72f6e1b 288 else if (feedback>130 && feedback <= 140){
Yolandataniaa 1:0122c72f6e1b 289 arm1.speed(0.075*pwm1);
Yolandataniaa 1:0122c72f6e1b 290 }
Yolandataniaa 1:0122c72f6e1b 291 else if (feedback>115 && feedback <= 130){
Yolandataniaa 1:0122c72f6e1b 292 arm1.speed(0.65*pwm1);
Yolandataniaa 1:0122c72f6e1b 293 }
Yolandataniaa 1:0122c72f6e1b 294 else {
Yolandataniaa 1:0122c72f6e1b 295 arm1.speed(pwm1);
Yolandataniaa 1:0122c72f6e1b 296 }
Yolandataniaa 1:0122c72f6e1b 297 }
Yolandataniaa 1:0122c72f6e1b 298 else if (error<0){
Yolandataniaa 1:0122c72f6e1b 299 //kode turun
Yolandataniaa 1:0122c72f6e1b 300 if (feedback>20 && feedback< 40 && target <= 20){
Yolandataniaa 1:0122c72f6e1b 301 arm1.speed(0);
Yolandataniaa 1:0122c72f6e1b 302 arm1.brake();
Yolandataniaa 1:0122c72f6e1b 303 }
Yolandataniaa 1:0122c72f6e1b 304 else if (feedback>40 && feedback <= 60){
Yolandataniaa 1:0122c72f6e1b 305 arm1.speed(0.1*pwm1);
Yolandataniaa 1:0122c72f6e1b 306 }
Yolandataniaa 1:0122c72f6e1b 307 else if (feedback>60 && feedback <= 90){
Yolandataniaa 1:0122c72f6e1b 308 arm1.speed(0.65*pwm1);
Yolandataniaa 1:0122c72f6e1b 309 }
Yolandataniaa 1:0122c72f6e1b 310 else {
Yolandataniaa 1:0122c72f6e1b 311 arm1.speed(pwm1);
Yolandataniaa 1:0122c72f6e1b 312 }
Yolandataniaa 1:0122c72f6e1b 313 }
Yolandataniaa 1:0122c72f6e1b 314 }