Spirit Dagozilla / Mbed 2 deprecated tune_PID

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed PID_buat_test_PID CMPS11_Serial_buat_test_PID encoder_buat_test_PID

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers setpoint_motor.cpp Source File

setpoint_motor.cpp

00001 #include "mbed.h"
00002 #include "motor.h"
00003 #include "PID.h"
00004 #include "encoderKRAI.h"
00005 
00006 
00007 // Serial
00008 Serial pc(USBTX,USBRX);
00009 
00010 ////DIG_R, DIG_L, PWM
00011 //motor motor1 (PA_9, PB_10, PA_8);
00012 //motor motor2 (PA_7, PB_6, PC_7);
00013 //motor motor3 (PA_5, PA_6, PC_8);
00014 
00015 ////Motor PG45 F7
00016 //motor motor1 (PB_6, PA_6, PA_7);
00017 //motor motor2 (PB_8, PC_7, PA_5);
00018 //motor motor3 (PB_9, PA_10, PB_10);
00019 
00020 //Motor PG45 Kiper
00021 motor motor1 (PA_12, PB_7);
00022 motor motor2 (PA_10, PB_8);
00023 motor motor3 (PH_1 , PB_6);
00024 
00025 ////Motor CRobot Calman
00026 //encoderKRAI enc1(PA_9, PA_8, 196  , encoderKRAI::X4_ENCODING); // M1 PPR Total = PPR Encoder * Mode Encoding * Gear Reduction 
00027 //encoderKRAI enc2(PA_1, PA_0, 196  , encoderKRAI::X4_ENCODING); // M2 PPR Total = PPR Encoder * Mode Encoding * Gear Reduction 
00028 //encoderKRAI enc3(PB_5 , PB_4, 196  , encoderKRAI::X4_ENCODING); // M3 PPR Total = PPR Encoder * Mode Encoding * Gear Reduction 
00029 
00030 //Encoder Kiper
00031 encoderKRAI enc1(PA_11, PB_12, 537.6  , encoderKRAI::X4_ENCODING); // M1 PPR Total = PPR Encoder * Mode Encoding * Gear Reduction 
00032 encoderKRAI enc2(PC_4, PB_13, 537.6  , encoderKRAI::X4_ENCODING); // M3 PPR Total = PPR Encoder * Mode Encoding * Gear Reduction
00033 encoderKRAI enc3(PD_2, PC_8, 537.6  , encoderKRAI::X4_ENCODING); // M2 PPR Total = PPR Encoder * Mode Encoding * Gear Reduction 
00034  //CHA, CHB
00035 //Encoder Motor PG45 F7
00036 //encoderKRAI enc1(PG_14, PD_10, 537.6  , encoderKRAI::X4_ENCODING); // M1 PPR Total = PPR Encoder * Mode Encoding * Gear Reduction 
00037 //encoderKRAI enc3(PF_1, PF_2, 537.6  , encoderKRAI::X4_ENCODING); // M2 PPR Total = PPR Encoder * Mode Encoding * Gear Reduction 
00038 //encoderKRAI enc2(PE_0 , PG_8, 537.6  , encoderKRAI::X4_ENCODING); // M3 PPR Total = PPR Encoder * Mode Encoding * Gear Reduction
00039 //
00040 ////Encoder Motor PG36 pake pin yang timer
00041 //encoderKRAI enc1(PA_0, PA_1, 537.6  , encoderKRAI::X4_ENCODING); // M1 PPR Total = PPR Encoder * Mode Encoding * Gear Reduction 
00042 //encoderKRAI enc2(PB_4, PB_5, 537.6  , encoderKRAI::X4_ENCODING); // M2 PPR Total = PPR Encoder * Mode Encoding * Gear Reduction 
00043 //encoderKRAI enc3(PB_6 , PB_7, 537.6  , encoderKRAI::X4_ENCODING); // M3 PPR Total = PPR Encoder * Mode Encoding * Gear Reduction  
00044 
00045 //Penggiring 
00046 //motor peng_l (PB_0, PA_4, PA_10); 
00047 //motor peng_r (PA_1, PA_0, PB_4);
00048 //
00049 //encoderKRAI enc_peng_l(PC_4, PB_13, 537.6 , encoderKRAI::X4_ENCODING); 
00050 //encoderKRAI enc_peng_r(PB_14, PB_15, 537.6 , encoderKRAI::X4_ENCODING);
00051 
00052 //Encoder External
00053 //encoderKRAI ext1 (PC_5, PC_6, 1440 , encoderKRAI::X4_ENCODING); // M1 PPR Total = PPR Encoder * Mode Encoding * Gear Reduction
00054 //encoderKRAI ext2 (PA_11, PC_9, 1440 , encoderKRAI::X4_ENCODING); // M1 PPR Total = PPR Encoder * Mode Encoding * Gear Reduction
00055 //encoderKRAI ext3 (PB_12, PB_2, 1440 , encoderKRAI::X4_ENCODING); // M1 PPR Total = PPR Encoder * Mode Encoding * Gear Reduction
00056 
00057 //ROBOT G
00058 //pid pid1(0.297430242013194, 9.0211605196814, 0, 100, 0.02);
00059 //pid pid2(0.307343953442149, 9.03308447439312, 0, 100, 0.02);
00060 //pid pid3(0.310277988423672, 8.54558353423818, 0, 100, 0.02);
00061 //pid pidL(0.30374, 7.4118, 0, 100, 0.02); //PID pidL(0.397778686128279, 5.7550366964209, 0, 100, 0.02);
00062 //pid pidR(0.27384, 7.3262, 0, 100, 0.02); //PID pidR(0.30272568701543, 6.0243755539975, 0, 100, 0.02);
00063 
00064 // Variabel rotasi ( dalam radian ) posisi, Kecepatan dan pid 
00065 float rot1,rot2,rot3, rotL, rotR;
00066 double v[5],vset[5],pidx[5],vold[5];
00067 double xrobot,yrobot,teta,xworld,yworld ;
00068 
00069 float PI = 3.14159 ;
00070 
00071 //variabel jarak titik tengah robot ke roda
00072 float d = 20.38 ;
00073 
00074 double v_l[1000],v_r[1000]; //Jarak linear
00075 
00076 double v_1[1000],v_2[1000], v_3[1000] , v_4[1000] , v_5[1000], v_6[1000];
00077 
00078 double Ts = 0.005 ; //ms
00079 double r = 0.05 ;//m
00080 double Pi = 3.14159265359 ; 
00081 
00082 int i,delay[1000],oldtime  ;
00083 
00084 double Setpoint[1000];
00085 
00086 Timer t; 
00087 
00088 /*****************
00089   Main
00090 ******************/
00091 int main()
00092 {
00093     wait(2);
00094     //pc.printf("CLEARDATA");
00095     //pc.printf("\n");
00096     
00097     for(int z = 0 ; z <5 ;z++){
00098     pc.printf("Setpoint,V_1,V_2,V_3,V_L,V_R,delay (ms)");
00099     pc.printf("\n");
00100     
00101     t.start();
00102     oldtime = 0 ;
00103     
00104     //inisialisasi bacaan
00105     v[0] = 0;
00106     v[1] = 0;
00107     v[2] = 0;
00108     
00109 
00110     for ( int j = 0; j < 11; j = j+10)
00111     {
00112         for ( i = 0 ; i < 300; i++ )
00113         {
00114             
00115             
00116             
00117                 motor1.setpwm((float) j/10.0);
00118                 motor2.setpwm((float) j/10.0);        
00119                 motor3.setpwm((float) j/10.0);
00120 //                peng_l.setpwm((float) j/10.0);
00121 //                peng_r.setpwm((float) j/10.0);
00122                 
00123                 v[0] = enc1.getRevolutions() * 2 * Pi * 0.024 / Ts ; 
00124                 v[1] = enc2.getRevolutions() * 2 * Pi * 0.024 / Ts ;
00125                 v[2] = enc3.getRevolutions() * 2 * Pi * 0.024 / Ts ; 
00126 //                v[3] = ext1.getRevolutions() * 2 * Pi * 0.024 / Ts ;
00127 //                v[4] = ext2.getRevolutions() * 2 * Pi * 0.024 / Ts ;
00128 //                v[5] = ext3.getRevolutions() * 2 * Pi * 0.024 / Ts ; 
00129 //                v[3] = enc_peng_l.getRevolutions() * 2 * Pi * 0.03125 / Ts ;
00130 //                v[4] = enc_peng_r.getRevolutions() * 2 * Pi * 0.03125 / Ts ; 
00131                 
00132                 
00133  //           }
00134             
00135             
00136             //Output kecepatan dalam m/s
00137             Setpoint[i] = (float) j/10.0;
00138             v_1[i] = v[0];
00139             v_2[i] = v[1];
00140             v_3[i] = v[2];
00141 //            v_4[i] = v[3];
00142 //            v_5[i] = v[4];
00143 //            v_6[i] = v[5];
00144 //            v_l[i] = v[3];
00145 //            v_r[i] = v[4];
00146             delay[i] = t.read_ms() - oldtime;
00147             oldtime = t.read_ms();
00148             wait_ms(Ts*1000);
00149             
00150         }
00151         
00152         
00153             
00154         t.reset();
00155         
00156         motor1.setpwm(0);
00157         motor2.setpwm(0);        
00158         motor3.setpwm(0);
00159 //        peng_l.setpwm(0);
00160 //        peng_r.setpwm(0);
00161 
00162             for ( i = 0 ; i < 300 ; i++ )
00163         {
00164             //Delay dalam ms
00165             //Jangan lupa bagi s_l dan s_r dibagi 100000 di excel
00166             //pc.printf("DATA,TIME,%d,%d,%d",int(v_l[i]*100000),int(v_r[i]*100000),delay[i]); 
00167             pc.printf("%.2f,%d,%d,%d,%d,%d,%d,%d",Setpoint[i],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]); 
00168             pc.printf("\n");
00169         }
00170     
00171         
00172     }
00173     wait(3);
00174     }
00175 
00176     while(1)
00177     {
00178         
00179     }   
00180 }
Generated on Sun Jul 31 2022 06:35:15 by  doxygen 1.7.2