main.cpp

00001 #include "mbed.h"
00002 #include "RF24.h"
00003 #include <cstdlib>
00004 
00005 //HIGH LOW Function
00006 int low = 0;
00007 int high = 1;
00008 
00009 //MAP Function
00010 long map(long x, long in_min, long in_max, long out_min, long out_max)
00011 {
00012   return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
00013 }
00014 
00015 //NRF24 Setting
00016 const uint64_t  address = 0xF0F0F0F0F0F0F001L;
00017 RF24            radio(PB_5, PB_4, PB_3, PB_7, PB_6);    // mosi, miso, sck, csn, ce, (irq not used)
00018 
00019 struct Data_Package {
00020     uint8_t LX;
00021     uint8_t LY;
00022     uint8_t RX;
00023     uint8_t RY;
00024     uint8_t XButton;
00025     uint8_t TButton;
00026     uint8_t SButton;
00027     uint8_t CButton;
00028     uint8_t UButton;
00029     uint8_t DButton;
00030     uint8_t LButton;
00031     uint8_t RButton;
00032     uint8_t L1;
00033     uint8_t L2;
00034     uint8_t L3;
00035     uint8_t R1;
00036     uint8_t R2;
00037     uint8_t R3;
00038 };
00039 Data_Package data;
00040 
00041 //Motor Variable
00042 int rpm_y = 0, rpm_x = 0;
00043 float rpm1, rpm2, rpm3, rpm4, rotateSpeed;
00044 int setRPM1 = 0, setRPM2 = 0, setRPM3 = 0, setRPM4 = 0;
00045 float max_rpm = 200 , pwm_freq = 0.0001;
00046 int currentRPM1,currentRPM2,currentRPM3,currentRPM4,currentRPM5,currentRPM6;
00047 int encCount1 = 540;
00048 int encCount2 = 540;
00049 int encCount3 = 540;
00050 int encCount4 = 540;
00051 
00052 float dataGyro = 0, yawTarget = 0;
00053 
00054 //PID_1
00055 float PIDValue1 = 0, gainValue1 = 0, pwmValue1 = 0;
00056 float error1, prevError1;
00057 float P_1 = 0, I_1 = 0, D_1 = 0;
00058 float Kp1 = 0.0005, Ki1 = 0, Kd1 = 0; //Atur
00059 
00060 //PID_2
00061 float PIDValue2 = 0, gainValue2 = 0, pwmValue2 = 0;
00062 float error2, prevError2;
00063 float P_2 = 0, I_2 = 0, D_2 = 0;
00064 float Kp2 = 0.0005, Ki2 = 0, Kd2 = 0; //Atur
00065 
00066 //PID_3
00067 float PIDValue3 = 0, gainValue3 = 0, pwmValue3 = 0;
00068 float error3, prevError3;
00069 float P_3 = 0, I_3 = 0, D_3 = 0;
00070 float Kp3 = 0.0005, Ki3 = 0, Kd3 = 0; //Atur
00071 
00072 //PID_4
00073 float PIDValue4 = 0, gainValue4 = 0, pwmValue4 = 0;
00074 float error4, prevError4;
00075 float P_4 = 0, I_4 = 0, D_4 = 0;
00076 float Kp4 = 0.0005, Ki4 = 0, Kd4 = 0; //Atur
00077 
00078 //Serial Communication
00079 Serial serial1(PA_9,PA_10); //TX,RX
00080 
00081 //Interrupt Declaration
00082 InterruptIn enc1a(PE_8);
00083 InterruptIn enc1b(PE_9);
00084 InterruptIn enc2a(PE_10);
00085 InterruptIn enc2b(PE_11);
00086 InterruptIn enc3a(PA_0);
00087 InterruptIn enc3b(PA_1);
00088 InterruptIn enc4a(PA_2);
00089 InterruptIn enc4b(PA_3);
00090 InterruptIn enc5a(PD_12);
00091 InterruptIn enc5b(PD_13);
00092 InterruptIn enc6a(PD_14);
00093 InterruptIn enc6b(PD_15);
00094 
00095 //Motor PIN
00096 PwmOut pwm1(PB_0);
00097 PwmOut pwm2(PB_1);
00098 PwmOut pwm3(PB_8);
00099 PwmOut pwm4(PB_9);
00100 
00101 DigitalOut ena1a(PC_4);
00102 DigitalOut ena1b(PA_4);
00103 DigitalOut ena2a(PA_5);
00104 DigitalOut ena2b(PC_5);
00105 DigitalOut ena3a(PD_7); //ext11 rubah
00106 DigitalOut ena3b(PE_1);
00107 DigitalOut ena4a(PE_0);
00108 DigitalOut ena4b(PD_3); //ext10 rubah
00109 
00110 AnalogIn mcs1(PC_0);
00111 AnalogIn mcs2(PC_1);
00112 AnalogIn mcs3(PC_2);
00113 AnalogIn mcs4(PC_3);
00114 
00115 Timer t;
00116 
00117 //Encoder PIN & Variable
00118 int rev1a = 0;
00119 int rev2a = 0;
00120 int rev3a = 0;
00121 int rev4a = 0;
00122 int rev5a = 0;
00123 int rev6a = 0;
00124 int rev1b = 0;
00125 int rev2b = 0;
00126 int rev3b = 0;
00127 int rev4b = 0;
00128 int rev5b = 0;
00129 int rev6b = 0;
00130 
00131 //100ms Timer
00132 float currentMillis = 0, prevMillis = 0;
00133 
00134 void incr1a(){
00135     rev1a=rev1a+1;
00136 }
00137 void incr1b(){
00138     rev1b=rev1b+1;
00139 }
00140 
00141 void incr2a(){
00142     rev2a=rev2a+1;
00143 }
00144 void incr2b(){
00145     rev2b=rev2b+1;
00146 }
00147 
00148 void incr3a(){
00149     rev3a=rev3a+1;
00150 }
00151 void incr3b(){
00152     rev3b=rev3b+1;
00153 }
00154 
00155 void incr4a(){
00156     rev4a=rev4a+1;
00157 }
00158 void incr4b(){
00159     rev4b=rev4b+1;
00160 }
00161 
00162 void incr5a(){
00163     rev5a=rev5a+1;
00164 }
00165 void incr5b(){
00166     rev5b=rev5b+1;
00167 }
00168 
00169 void incr6a(){
00170     rev6a=rev6a+1;
00171 }
00172 void incr6b(){
00173     rev6b=rev6b+1;
00174 }
00175 
00176 void bacaRemote() {
00177   //Cek adakah input dari controller
00178     if (data.LX != 128 || data.LY != 128 || data.RX != 128) {
00179         //Move
00180         if ((data.LY != 128 || data.LX != 128) && data.RX == 128) {
00181             //serial1.printf("Move : %d  %d\n", data.LX, data.LY);
00182             setRPM1 = rpm_y - rpm_x;
00183             setRPM2 = rpm_y + rpm_x;
00184             setRPM3 = rpm_y + rpm_x;
00185             setRPM4 = rpm_y - rpm_x;
00186         }
00187 
00188         //Rotate
00189         else if (data.LY == 128 && data.LX == 128 && data.RX != 128) {
00190             //serial1.printf("Putar Kiri : %f\n", rotateSpeed);
00191             setRPM1 = -rotateSpeed;
00192             setRPM2 = -rotateSpeed;
00193             setRPM3 = rotateSpeed;
00194             setRPM4 = rotateSpeed;
00195             yawTarget = dataGyro;
00196         }
00197     }
00198 
00199     //Stop Motor
00200     if (data.LY == 128 && data.LX == 128 && data.RX == 128) {
00201         setRPM1 = 0;
00202         setRPM2 = 0;
00203         setRPM3 = 0;
00204         setRPM4 = 0;
00205         //serial1.printf("Stop\n");
00206     }
00207 }
00208 
00209 //PID MOTOR 1
00210 void PID1(){
00211     error1 = abs(setRPM1)-currentRPM1;
00212     P_1 = error1;
00213     I_1 = I_1 + error1;
00214     D_1 = error1 - prevError1;
00215     
00216     if (I_1 > 1){
00217         I_1 = 1;
00218     }
00219         
00220     if (I_1 < 0){
00221         I_1 = 0;
00222     }
00223     
00224     PIDValue1 = (Kp1 * P_1) + (Ki1 * P_1);
00225     prevError1 = error1;
00226     pwmValue1 = pwmValue1 + PIDValue1;
00227     
00228     if(pwmValue1 >= 1){
00229         pwmValue1 = 1;
00230     }
00231     
00232     if(pwmValue1 < 0){
00233         pwmValue1 = 0;
00234     }
00235     
00236     if(setRPM1 == 0){
00237         pwmValue1 = 0;
00238         pwm1.period(pwm_freq);
00239         pwm1.write(0.5);
00240         ena1a=low;
00241         ena1b=low;
00242     }   
00243     
00244     else if(setRPM1 > 0){
00245         pwm1.period(pwm_freq);
00246         pwm1.write(pwmValue1);
00247         ena1a=high;
00248         ena1b=low;
00249     }
00250     
00251     else{
00252         pwm1.period(pwm_freq);
00253         pwm1.write(pwmValue1);
00254         ena1a=low;
00255         ena1b=high;    
00256     }
00257 }
00258 
00259 //PID MOTOR 2
00260 void PID2(){
00261     error2 = abs(setRPM2)-currentRPM2;
00262     P_2 = error2;
00263     I_2 = I_2 + error2;
00264     D_2 = error2 - prevError2;
00265     
00266     if (I_2 > 100){
00267         I_2 = 100;
00268     }
00269         
00270     if (I_2 < 0){
00271         I_2 = 0;
00272     }
00273     
00274     PIDValue2 = (Kp2 * P_2) + (Ki2 * P_2);
00275     prevError2 = error2;
00276     pwmValue2 = pwmValue2 + PIDValue2;
00277     
00278     if(pwmValue2 >= 100){
00279         pwmValue2 = 100;
00280     }
00281     
00282     if(pwmValue2 < 0){
00283         pwmValue2 = 0;
00284     }
00285     
00286     if(setRPM2 == 0){
00287         pwmValue2 = 0;
00288         pwm2.period(pwm_freq);
00289         pwm2.write(0.8);
00290         ena2a=low;
00291         ena2b=low;
00292     }   
00293     
00294     else if(setRPM2 > 0){
00295         pwm2.period(pwm_freq);
00296         pwm2.write(pwmValue1);
00297         ena2a=high;
00298         ena2b=low;
00299     }
00300     
00301     else{
00302         pwm2.period(pwm_freq);
00303         pwm2.write(pwmValue1);
00304         ena2a=low;
00305         ena2b=high;    
00306     }
00307 }
00308 
00309 //PID MOTOR 3
00310 void PID3(){
00311     error3 = abs(setRPM3)-currentRPM3;
00312     P_3 = error3;
00313     I_3 = I_3 + error3;
00314     D_3 = error3 - prevError3;
00315     
00316     if (I_3 > 100){
00317         I_3 = 100;
00318     }
00319         
00320     if (I_3 < 0){
00321         I_3 = 0;
00322     }
00323     
00324     PIDValue3 = (Kp3 * P_3) + (Ki3 * P_3);
00325     prevError3 = error3;
00326     pwmValue3 = pwmValue3 + PIDValue3;
00327     
00328     if(pwmValue3 >= 100){
00329         pwmValue3 = 100;
00330     }
00331     
00332     if(pwmValue3 < 0){
00333         pwmValue3 = 0;
00334     }
00335     
00336     if(setRPM3 == 0){
00337         pwmValue3 = 0;
00338         pwm3.period(pwm_freq);
00339         pwm3.write(0.1);
00340         ena3a=low;
00341         ena3b=low;
00342     }   
00343     
00344     else if(setRPM3 > 0){
00345         pwm3.period(pwm_freq);
00346         pwm3.write(pwmValue3);
00347         ena3a=high;
00348         ena3b=low;
00349     }
00350     
00351     else{
00352         pwm3.period(pwm_freq);
00353         pwm3.write(pwmValue3);
00354         ena3a=low;
00355         ena3b=high;    
00356     }
00357 }
00358 
00359 //PID MOTOR 4
00360 void PID4(){
00361     error4 = abs(setRPM4)-currentRPM4;
00362     P_4 = error4;
00363     I_4 = I_4 + error4;
00364     D_4 = error4 - prevError4;
00365     
00366     if (I_4 > 100){
00367         I_4 = 100;
00368     }
00369         
00370     if (I_4 < 0){
00371         I_4 = 0;
00372     }
00373     
00374     PIDValue4 = (Kp4 * P_4) + (Ki4 * P_4);
00375     prevError4 = error4;
00376     pwmValue4 = pwmValue4 + PIDValue4;
00377     
00378     if(pwmValue4 >= 100){
00379         pwmValue4 = 100;
00380     }
00381     
00382     if(pwmValue4 < 0){
00383         pwmValue4 = 0;
00384     }
00385     
00386     if(setRPM4 == 0){
00387         pwm4.period(pwm_freq);
00388         pwmValue4 = 0;
00389         pwm4.write(0.3);
00390         ena4a=low;
00391         ena4b=low;
00392     }   
00393     
00394     else if(setRPM4 > 0){
00395         pwm4.period(pwm_freq);
00396         pwm4.write(pwmValue4);
00397         ena4a=high;
00398         ena4b=low;
00399     }
00400     
00401     else{
00402         pwm4.period(pwm_freq);
00403         pwm4.write(pwmValue4);
00404         ena4a=low;
00405         ena4b=high;    
00406     }
00407 }
00408 
00409 
00410 int main(void)
00411 {
00412     serial1.baud(2000000);
00413     radio.begin();
00414     radio.openReadingPipe(0, address);
00415     radio.setAutoAck(false);
00416     radio.setDataRate(RF24_250KBPS);
00417     radio.setPALevel(RF24_PA_MIN);
00418     radio.startListening();
00419     
00420     data.LX = 128;
00421     data.LY = 128;
00422     data.RX = 128;
00423     data.RY = 128;
00424     data.XButton = 0;
00425     data.TButton = 0;
00426     data.SButton = 0;
00427     data.CButton = 0;
00428     data.UButton = 0;
00429     data.DButton = 0;
00430     data.LButton = 0;
00431     data.RButton = 0;
00432     data.L1 = 0;
00433     data.L2 = 0;
00434     data.L3 = 0;
00435     data.R1 = 0;
00436     data.R2 = 0;
00437     data.R3 = 0;
00438     
00439     //Rising Edge
00440     enc1a.rise(&incr1a);
00441     enc1b.rise(&incr1b);
00442     enc2a.rise(&incr2a);
00443     enc2b.rise(&incr2b);
00444     enc3a.rise(&incr3a);
00445     enc3b.rise(&incr3b);
00446     enc4a.rise(&incr4a);
00447     enc4b.rise(&incr4b);
00448     enc5a.rise(&incr5a);
00449     enc5b.rise(&incr5b);
00450     enc6a.rise(&incr6a);
00451     enc6b.rise(&incr6b);
00452     //Falling Edge
00453     enc1a.fall(&incr1a);
00454     enc1b.fall(&incr1b);
00455     enc2a.fall(&incr2a);
00456     enc2b.fall(&incr2b);
00457     enc3a.fall(&incr3a);
00458     enc3b.fall(&incr3b);
00459     enc4a.fall(&incr4a);
00460     enc4b.fall(&incr4b);
00461     enc5a.fall(&incr5a);
00462     enc5b.fall(&incr5b);
00463     enc6a.fall(&incr6a);
00464     enc6b.fall(&incr6b);
00465     
00466     t.start();
00467     prevMillis = 0 + t.read();
00468     
00469     while (1) {
00470         if (radio.available()) {
00471             radio.read(&data, sizeof(Data_Package)); // read message and send acknowledge back to the master
00472         }
00473         
00474         currentMillis = 0 + t.read();
00475         if(currentMillis - prevMillis >= 0.099992){
00476             prevMillis = currentMillis;
00477             
00478             currentRPM1 = (rev1a + rev1b) * 600 / encCount1;
00479             currentRPM2 = (rev2a + rev2b) * 600 / encCount2;
00480             currentRPM3 = (rev3a + rev3b) * 600 / encCount3;
00481             currentRPM4 = (rev4a + rev4b) * 600 / encCount4;
00482             currentRPM5 = (rev5a + rev5b) * 600 / 540;
00483             currentRPM6 = (rev6a + rev6b) * 600 / 540;
00484             
00485             
00486             PID1();
00487             PID2();
00488             PID3();
00489             PID4();
00490             
00491             rev1a=0;
00492             rev2a=0;
00493             rev3a=0;
00494             rev4a=0;
00495             rev1b=0;
00496             rev2b=0;
00497             rev3b=0;
00498             rev4b=0;
00499             
00500             serial1.printf("%d  %d  %d  %d\n", currentRPM1, currentRPM2, currentRPM3, currentRPM4);
00501         }
00502 
00503         rpm_x = (map(data.LX, 0, 255, -100, 100) * max_rpm) / 100;
00504         rpm_y = (map(data.LY, 0, 255, -100, 100) * max_rpm) / 100;
00505         rotateSpeed = (map(data.RX, 0, 255, -100, 100) * max_rpm) / 200;
00506         bacaRemote();
00507     }
00508 }