Dagozilla to RoboCup / Mbed 2 deprecated test_PID

buat data logging PID motor

Dependencies:   CMPS11_Serial_buat_test_PID PID_buat_test_PID encoder_buat_test_PID mbed

Fork of test_PID by Dagozilla to RoboCup

setpoint_motor.cpp@4:e330d9c25dc4, 2018-11-02 (annotated)

Committer:
alienbernamaihsan
Date:
Fri Nov 02 11:13:26 2018 +0000
Revision:
4:e330d9c25dc4
Parent:
2:c504b65348ef 
error import

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alienbernamaihsan 2:c504b65348ef 1 #include "mbed.h"
alienbernamaihsan 2:c504b65348ef 2 #include "motor.h"
alienbernamaihsan 2:c504b65348ef 3 #include "PID.h"
alienbernamaihsan 2:c504b65348ef 4 #include "encoderKRAI.h"
alienbernamaihsan 2:c504b65348ef 5
alienbernamaihsan 2:c504b65348ef 6
alienbernamaihsan 2:c504b65348ef 7 // Serial
alienbernamaihsan 2:c504b65348ef 8 Serial pc(USBTX,USBRX);
alienbernamaihsan 2:c504b65348ef 9
alienbernamaihsan 2:c504b65348ef 10 //DIG_R, DIG_L, PWM
alienbernamaihsan 2:c504b65348ef 11 motor motor1 (PB_10, PA_9, PA_8); //M3
alienbernamaihsan 2:c504b65348ef 12 motor motor2 (PB_6, PA_7, PC_7); //M2
alienbernamaihsan 2:c504b65348ef 13 motor motor3 (PA_6, PA_5, PC_8); //M1
alienbernamaihsan 2:c504b65348ef 14
alienbernamaihsan 2:c504b65348ef 15 // CHA, CHB
alienbernamaihsan 2:c504b65348ef 16 //Encoder Motor PG36
alienbernamaihsan 2:c504b65348ef 17 encoderKRAI enc1(PC_9, PB_2, 537.6 , encoderKRAI::X4_ENCODING); // M1 PPR Total = PPR Encoder * Mode Encoding * Gear Reduction
alienbernamaihsan 2:c504b65348ef 18 encoderKRAI enc2(PA_11, PA_12, 537.6 , encoderKRAI::X4_ENCODING); // M2 PPR Total = PPR Encoder * Mode Encoding * Gear Reduction
alienbernamaihsan 2:c504b65348ef 19 encoderKRAI enc3(PC_5 , PC_6, 537.6 , encoderKRAI::X4_ENCODING); // M3 PPR Total = PPR Encoder * Mode Encoding * Gear Reduction
alienbernamaihsan 2:c504b65348ef 20
alienbernamaihsan 2:c504b65348ef 21 //Penggiring
alienbernamaihsan 2:c504b65348ef 22 //motor peng_l (PA_4, PB_0, PA_10);
alienbernamaihsan 2:c504b65348ef 23 //motor peng_r (PA_1, PA_0, PB_4);
alienbernamaihsan 2:c504b65348ef 24 //encoderKRAI enc_peng_l(PB_13, PC_4, 537.6 , encoderKRAI::X4_ENCODING);
alienbernamaihsan 2:c504b65348ef 25 //encoderKRAI enc_peng_r(PB_14, PB_15, 537.6 , encoderKRAI::X4_ENCODING);
alienbernamaihsan 2:c504b65348ef 26
alienbernamaihsan 2:c504b65348ef 27
alienbernamaihsan 2:c504b65348ef 28 //ROBOT G
alienbernamaihsan 2:c504b65348ef 29 pid pid1(0.297430242013194, 9.0211605196814, 0, 100, 0.02);
alienbernamaihsan 2:c504b65348ef 30 pid pid2(0.307343953442149, 9.03308447439312, 0, 100, 0.02);
alienbernamaihsan 2:c504b65348ef 31 pid pid3(0.310277988423672, 8.54558353423818, 0, 100, 0.02);
alienbernamaihsan 2:c504b65348ef 32 //pid pidL(0.30374, 7.4118, 0, 100, 0.02); //PID pidL(0.397778686128279, 5.7550366964209, 0, 100, 0.02);
alienbernamaihsan 2:c504b65348ef 33 //pid pidR(0.27384, 7.3262, 0, 100, 0.02); //PID pidR(0.30272568701543, 6.0243755539975, 0, 100, 0.02);
alienbernamaihsan 2:c504b65348ef 34
alienbernamaihsan 2:c504b65348ef 35 // Variabel rotasi ( dalam radian ) posisi, Kecepatan dan pid
alienbernamaihsan 2:c504b65348ef 36 float rot1,rot2,rot3, rotL, rotR;
alienbernamaihsan 2:c504b65348ef 37 double v[5],vset[5],pidx[5],vold[5];
alienbernamaihsan 2:c504b65348ef 38 double xrobot,yrobot,teta,xworld,yworld ;
alienbernamaihsan 2:c504b65348ef 39
alienbernamaihsan 2:c504b65348ef 40 float PI = 3.14159 ;
alienbernamaihsan 2:c504b65348ef 41
alienbernamaihsan 2:c504b65348ef 42 //variabel jarak titik tengah robot ke roda
alienbernamaihsan 2:c504b65348ef 43 float d = 20.38 ;
alienbernamaihsan 2:c504b65348ef 44
alienbernamaihsan 2:c504b65348ef 45 double v_l[1000],v_r[1000]; //Jarak linear
alienbernamaihsan 2:c504b65348ef 46
alienbernamaihsan 2:c504b65348ef 47 double v_1[1000],v_2[1000], v_3[1000];
alienbernamaihsan 2:c504b65348ef 48
alienbernamaihsan 2:c504b65348ef 49 double Ts = 0.02 ; //ms
alienbernamaihsan 2:c504b65348ef 50 double r = 0.05 ;//m
alienbernamaihsan 2:c504b65348ef 51 double Pi = 3.14159265359 ;
alienbernamaihsan 2:c504b65348ef 52
alienbernamaihsan 2:c504b65348ef 53 int i,delay[1000],oldtime ;
alienbernamaihsan 2:c504b65348ef 54
alienbernamaihsan 2:c504b65348ef 55 double Setpoint[1000];
alienbernamaihsan 2:c504b65348ef 56
alienbernamaihsan 2:c504b65348ef 57 Timer t;
alienbernamaihsan 2:c504b65348ef 58
alienbernamaihsan 2:c504b65348ef 59 /*****************
alienbernamaihsan 2:c504b65348ef 60 Main
alienbernamaihsan 2:c504b65348ef 61 ******************/
alienbernamaihsan 2:c504b65348ef 62 int main()
alienbernamaihsan 2:c504b65348ef 63 {
alienbernamaihsan 2:c504b65348ef 64 wait(3);
alienbernamaihsan 2:c504b65348ef 65 pc.printf("CLEARDATA");
alienbernamaihsan 2:c504b65348ef 66 pc.printf("\n");
alienbernamaihsan 2:c504b65348ef 67
alienbernamaihsan 2:c504b65348ef 68 pc.printf("LABEL,Time,Setpoint,V_1,V_2,V_3,V_L,V_R,delay (ms)");
alienbernamaihsan 2:c504b65348ef 69 pc.printf("\n");
alienbernamaihsan 2:c504b65348ef 70
alienbernamaihsan 2:c504b65348ef 71 t.start();
alienbernamaihsan 2:c504b65348ef 72 oldtime = 0 ;
alienbernamaihsan 2:c504b65348ef 73
alienbernamaihsan 2:c504b65348ef 74
alienbernamaihsan 2:c504b65348ef 75 for ( i = 0 ; i < 1000 ; i++ )
alienbernamaihsan 2:c504b65348ef 76 {
alienbernamaihsan 2:c504b65348ef 77 if ( (i >= 100) && (i < 400) )
alienbernamaihsan 2:c504b65348ef 78 {
alienbernamaihsan 2:c504b65348ef 79 vset[0] = 0.5; //Locomotion
alienbernamaihsan 2:c504b65348ef 80 vset[1] = 0.5; //Locomotion
alienbernamaihsan 2:c504b65348ef 81 vset[2] = 0.5; //Locomotion//
alienbernamaihsan 2:c504b65348ef 82 // vset[3] = 0.5; //Dribbler
alienbernamaihsan 2:c504b65348ef 83 // vset[4] = 0.5; //Dribbler
alienbernamaihsan 2:c504b65348ef 84
alienbernamaihsan 2:c504b65348ef 85 v[0] = enc1.getRevolutions() * 2 * Pi * 0.05 / Ts ;
alienbernamaihsan 2:c504b65348ef 86 v[1] = enc2.getRevolutions() * 2 * Pi * 0.05 / Ts ;
alienbernamaihsan 2:c504b65348ef 87 v[2] = enc3.getRevolutions() * 2 * Pi * 0.05 / Ts ; //
alienbernamaihsan 2:c504b65348ef 88 // v[3] = enc_peng_l.getRevolutions() * 2 * Pi * 0.03125 / Ts ;
alienbernamaihsan 2:c504b65348ef 89 // v[4] = enc_peng_r.getRevolutions() * 2 * Pi * 0.03125 / Ts ;
alienbernamaihsan 2:c504b65348ef 90
alienbernamaihsan 2:c504b65348ef 91 pidx[0] = pid1.createpwm(vset[0],v[0]) ;
alienbernamaihsan 2:c504b65348ef 92 pidx[1] = pid2.createpwm(vset[1],v[1]) ;
alienbernamaihsan 2:c504b65348ef 93 pidx[2] = pid3.createpwm(vset[2],v[2]) ;
alienbernamaihsan 2:c504b65348ef 94 // pidx[3] = pidL.createpwm(vset[3],v[3]) ;
alienbernamaihsan 2:c504b65348ef 95 // pidx[4] = pidR.createpwm(vset[4],v[4]) ;
alienbernamaihsan 2:c504b65348ef 96
alienbernamaihsan 2:c504b65348ef 97 motor1.setpwm(pidx[0]);
alienbernamaihsan 2:c504b65348ef 98 motor2.setpwm(pidx[1]);
alienbernamaihsan 2:c504b65348ef 99 motor3.setpwm(pidx[2]);//
alienbernamaihsan 2:c504b65348ef 100 // peng_l.setpwm(pidx[3]);
alienbernamaihsan 2:c504b65348ef 101 // peng_r.setpwm(pidx[4]);
alienbernamaihsan 2:c504b65348ef 102 }
alienbernamaihsan 2:c504b65348ef 103
alienbernamaihsan 2:c504b65348ef 104 if ( (i >= 400) && (i < 700) )
alienbernamaihsan 2:c504b65348ef 105 {
alienbernamaihsan 2:c504b65348ef 106 vset[0] = 1.0; //Locomotion
alienbernamaihsan 2:c504b65348ef 107 vset[1] = 1.0; //Locomotion
alienbernamaihsan 2:c504b65348ef 108 vset[2] = 1.0; //Locomotion
alienbernamaihsan 2:c504b65348ef 109 // vset[3] = 1.0; //Dribbler
alienbernamaihsan 2:c504b65348ef 110 // vset[4] = 1.0; //Dribbler
alienbernamaihsan 2:c504b65348ef 111
alienbernamaihsan 2:c504b65348ef 112 v[0] = enc1.getRevolutions() * 2 * Pi * 0.05 / Ts ;
alienbernamaihsan 2:c504b65348ef 113 v[1] = enc2.getRevolutions() * 2 * Pi * 0.05 / Ts ;
alienbernamaihsan 2:c504b65348ef 114 v[2] = enc3.getRevolutions() * 2 * Pi * 0.05 / Ts ;
alienbernamaihsan 2:c504b65348ef 115 // v[3] = enc_peng_l.getRevolutions() * 2 * Pi * 0.03125 / Ts ;
alienbernamaihsan 2:c504b65348ef 116 // v[4] = enc_peng_r.getRevolutions() * 2 * Pi * 0.03125 / Ts ;
alienbernamaihsan 2:c504b65348ef 117
alienbernamaihsan 2:c504b65348ef 118 pidx[0] = pid1.createpwm(vset[0],v[0]) ;
alienbernamaihsan 2:c504b65348ef 119 pidx[1] = pid2.createpwm(vset[1],v[1]) ;
alienbernamaihsan 2:c504b65348ef 120 pidx[2] = pid3.createpwm(vset[2],v[2]) ;
alienbernamaihsan 2:c504b65348ef 121 // pidx[3] = pidL.createpwm(vset[3],v[3]) ;
alienbernamaihsan 2:c504b65348ef 122 // pidx[4] = pidR.createpwm(vset[4],v[4]) ;
alienbernamaihsan 2:c504b65348ef 123
alienbernamaihsan 2:c504b65348ef 124 motor1.setpwm(pidx[0]);
alienbernamaihsan 2:c504b65348ef 125 motor2.setpwm(pidx[1]);
alienbernamaihsan 2:c504b65348ef 126 motor3.setpwm(pidx[2]);
alienbernamaihsan 2:c504b65348ef 127 // peng_l.setpwm(pidx[3]);
alienbernamaihsan 2:c504b65348ef 128 // peng_r.setpwm(pidx[4]);
alienbernamaihsan 2:c504b65348ef 129 }
alienbernamaihsan 2:c504b65348ef 130
alienbernamaihsan 2:c504b65348ef 131 if ( i >= 700 )
alienbernamaihsan 2:c504b65348ef 132 {
alienbernamaihsan 2:c504b65348ef 133 vset[0] = 1.5; //Locomotion
alienbernamaihsan 2:c504b65348ef 134 vset[1] = 1.5; //Locomotion
alienbernamaihsan 2:c504b65348ef 135 vset[2] = 1.5; //Locomotion
alienbernamaihsan 2:c504b65348ef 136 // vset[3] = 1.5; //Dribbler
alienbernamaihsan 2:c504b65348ef 137 // vset[4] = 1.5; //Dribbler
alienbernamaihsan 2:c504b65348ef 138
alienbernamaihsan 2:c504b65348ef 139 v[0] = enc1.getRevolutions() * 2 * Pi * 0.05 / Ts ;
alienbernamaihsan 2:c504b65348ef 140 v[1] = enc2.getRevolutions() * 2 * Pi * 0.05 / Ts ;
alienbernamaihsan 2:c504b65348ef 141 v[2] = enc3.getRevolutions() * 2 * Pi * 0.05 / Ts ;
alienbernamaihsan 2:c504b65348ef 142 // v[3] = enc_peng_l.getRevolutions() * 2 * Pi * 0.03125 / Ts ;
alienbernamaihsan 2:c504b65348ef 143 // v[4] = enc_peng_r.getRevolutions() * 2 * Pi * 0.03125 / Ts ;
alienbernamaihsan 2:c504b65348ef 144
alienbernamaihsan 2:c504b65348ef 145 pidx[0] = pid1.createpwm(vset[0],v[0]) ;
alienbernamaihsan 2:c504b65348ef 146 pidx[1] = pid2.createpwm(vset[1],v[1]) ;
alienbernamaihsan 2:c504b65348ef 147 pidx[2] = pid3.createpwm(vset[2],v[2]) ;
alienbernamaihsan 2:c504b65348ef 148 // pidx[3] = pidL.createpwm(vset[3],v[3]) ;
alienbernamaihsan 2:c504b65348ef 149 // pidx[4] = pidR.createpwm(vset[4],v[4]) ;
alienbernamaihsan 2:c504b65348ef 150
alienbernamaihsan 2:c504b65348ef 151 motor1.setpwm(pidx[0]);
alienbernamaihsan 2:c504b65348ef 152 motor2.setpwm(pidx[1]);
alienbernamaihsan 2:c504b65348ef 153 motor3.setpwm(pidx[2]);
alienbernamaihsan 2:c504b65348ef 154 // peng_l.setpwm(pidx[3]);
alienbernamaihsan 2:c504b65348ef 155 // peng_r.setpwm(pidx[4]);
alienbernamaihsan 2:c504b65348ef 156 }
alienbernamaihsan 2:c504b65348ef 157
alienbernamaihsan 2:c504b65348ef 158 //Output kecepatan dalam m/s
alienbernamaihsan 2:c504b65348ef 159 v_1[i] = v[0];
alienbernamaihsan 2:c504b65348ef 160 v_2[i] = v[1];
alienbernamaihsan 2:c504b65348ef 161 v_3[i] = v[2];
alienbernamaihsan 2:c504b65348ef 162 // v_l[i] = v[3];
alienbernamaihsan 2:c504b65348ef 163 // v_r[i] = v[4];
alienbernamaihsan 2:c504b65348ef 164 delay[i] = t.read_ms() - oldtime;
alienbernamaihsan 2:c504b65348ef 165 oldtime = t.read_ms();
alienbernamaihsan 2:c504b65348ef 166 wait_ms(Ts*1000);
alienbernamaihsan 2:c504b65348ef 167
alienbernamaihsan 2:c504b65348ef 168 }
alienbernamaihsan 2:c504b65348ef 169
alienbernamaihsan 2:c504b65348ef 170 for ( i = 0 ; i < 100 ; i++ )
alienbernamaihsan 2:c504b65348ef 171 {
alienbernamaihsan 2:c504b65348ef 172 Setpoint[i] = 0.0;
alienbernamaihsan 2:c504b65348ef 173 }
alienbernamaihsan 2:c504b65348ef 174
alienbernamaihsan 2:c504b65348ef 175 for ( i = 100 ; i < 400 ; i++ )
alienbernamaihsan 2:c504b65348ef 176 {
alienbernamaihsan 2:c504b65348ef 177 Setpoint[i] = 0.5;
alienbernamaihsan 2:c504b65348ef 178 }
alienbernamaihsan 2:c504b65348ef 179
alienbernamaihsan 2:c504b65348ef 180 for ( i = 400 ; i < 700 ; i++ )
alienbernamaihsan 2:c504b65348ef 181 {
alienbernamaihsan 2:c504b65348ef 182 Setpoint[i] = 1.0;
alienbernamaihsan 2:c504b65348ef 183 }
alienbernamaihsan 2:c504b65348ef 184
alienbernamaihsan 2:c504b65348ef 185 for ( i = 700 ; i < 1000 ; i++ )
alienbernamaihsan 2:c504b65348ef 186 {
alienbernamaihsan 2:c504b65348ef 187 Setpoint[i] = 1.5;
alienbernamaihsan 2:c504b65348ef 188 }
alienbernamaihsan 2:c504b65348ef 189
alienbernamaihsan 2:c504b65348ef 190 t.reset();
alienbernamaihsan 2:c504b65348ef 191
alienbernamaihsan 2:c504b65348ef 192 motor1.setpwm(0);
alienbernamaihsan 2:c504b65348ef 193 motor2.setpwm(0);
alienbernamaihsan 2:c504b65348ef 194 motor3.setpwm(0);
alienbernamaihsan 2:c504b65348ef 195 // peng_l.setpwm(0);
alienbernamaihsan 2:c504b65348ef 196 // peng_r.setpwm(0);
alienbernamaihsan 2:c504b65348ef 197
alienbernamaihsan 2:c504b65348ef 198 for ( i = 0 ; i < 1000 ; i++ )
alienbernamaihsan 2:c504b65348ef 199 {
alienbernamaihsan 2:c504b65348ef 200 //Delay dalam ms
alienbernamaihsan 2:c504b65348ef 201 //Jangan lupa bagi s_l dan s_r dibagi 100000 di excel
alienbernamaihsan 2:c504b65348ef 202 //pc.printf("DATA,TIME,%d,%d,%d",int(v_l[i]*100000),int(v_r[i]*100000),delay[i]);
alienbernamaihsan 2:c504b65348ef 203 pc.printf("DATA,TIME,%d,%d,%d,%d,%d,%d,%d",int(Setpoint[i]*10),int(v_1[i]*100000),int(v_2[i]*100000),int(v_3[i]*100000),int(v_l[i]*100000),int(v_r[i]*100000),delay[i]);
alienbernamaihsan 2:c504b65348ef 204 pc.printf("\n");
alienbernamaihsan 2:c504b65348ef 205 }
alienbernamaihsan 2:c504b65348ef 206 while(1)
alienbernamaihsan 2:c504b65348ef 207 {
alienbernamaihsan 2:c504b65348ef 208
alienbernamaihsan 2:c504b65348ef 209 }
alienbernamaihsan 2:c504b65348ef 210 }