main.cpp

00001 /*
00002  * Author       : Garudago ITB 2019
00003  * Developer    : Garudago ITB 2019
00004  * Maintainer   : Calmantara Sumpono Putra
00005  * Version      : 2.0.0
00006  */
00007 
00008 #include "mbed.h"
00009 #include "millis.h"
00010 #include <garudago_conf/robotpin.h>
00011 #include <garudago_conf/variable.h>
00012 #include <garudago_conf/config.h>
00013 #include <garudago_hardware_interface/actuator.h>
00014 #include <garudago_hardware_interface/sensor.h>
00015 #include <pid_dagoz/PID.h>
00016 
00017 PID locomotion_right_top(right_top_kp, 
00018                          right_top_ki,
00019                          right_top_kd, 
00020                          right_top_N, 
00021                          right_top_TS, 
00022                          right_top_FF, 
00023                          PID::PI_MODE);
00024 
00025 PID locomotion_left_top(left_top_kp, 
00026                         left_top_ki,
00027                         left_top_kd, 
00028                         left_top_N, 
00029                         left_top_TS, 
00030                         left_top_FF, 
00031                         PID::PI_MODE);
00032 
00033 PID locomotion_right_back(right_back_kp, 
00034                           right_back_ki,
00035                           right_back_kd, 
00036                           right_back_N, 
00037                           right_back_TS, 
00038                           right_back_FF, 
00039                           PID::PI_MODE);
00040 
00041 PID locomotion_left_back(left_back_kp, 
00042                          left_back_ki,
00043                          left_back_kd, 
00044                          left_back_N, 
00045                          left_back_TS, 
00046                          left_back_FF, 
00047                          PID::PI_MODE);
00048 
00049 Base base;
00050 Compass compass;
00051 
00052 void locomotionMovement();
00053 void shagaiMovement(int _state, float _theta, bool _up);
00054 void shagaiParameter();
00055 void pneumaticMovement();
00056 void stickState();
00057 
00058 int main(){
00059     stick.setup();
00060     compass.compass_reset((float)compass_sensor.getAngle()/10);
00061     startMillis();
00062     stick.idle();       //start stick
00063 
00064     //start robot geometry
00065     robot_geometry[0] = 0.0;
00066     robot_geometry[1] = 0.0;
00067     robot_geometry[2] = 0.0;
00068 
00069     while(1){
00070         if(stick.readable()){
00071         //procedure to read stick data
00072             stick.baca_data();
00073             stick.olah_data();
00074         }
00075         // if(millis() - prev_compass_timer > 50){
00076         // //before external encoder fixed, use faster sampling rate
00077         //     compass.compass_update((float)compass_sensor.getAngle()/10.0);
00078         //     robot_geometry[2] = compass.compass_value();
00079         //     prev_compass_timer = millis();
00080         // }
00081         if(millis() - prev_motor_timer > 10){
00082             stickState();
00083             locomotionMovement();
00084             //debug encoder direction
00085             // pc.printf("lt=%.2f rt=%.2f lb=%.2f rb=%.2f lt=%.2f rt=%.2f lb=%.2f rb=%.2f cmps=%.2f\n\r", left_top_vel, 
00086             //                                                                                             right_top_vel, 
00087             //                                                                                             left_back_vel, 
00088             //                                                                                             right_back_vel,
00089             //                                                                                             wheels_target_velocity[0], 
00090             //                                                                                             wheels_target_velocity[1],
00091             //                                                                                             wheels_target_velocity[2],
00092             //                                                                                             wheels_target_velocity[3],
00093             //                                                                                             robot_geometry[2]);
00094             prev_motor_timer = millis();
00095         }
00096         if(millis() - prev_pneumatic_timer > 15){
00097             shagaiParameter();
00098             pneumaticMovement();
00099             shagaiMovement(state_condition, theta_shagai, up);
00100             prev_pneumatic_timer = millis();
00101         }  
00102     }
00103 }
00104 
00105 /*Function and Procedure Declaration*/
00106 void locomotionMovement(){
00107     left_top_vel    = (enc_left_top.getPulses() * 2 * PI * WHEEL_RAD)/0.01;
00108     right_top_vel   = (enc_right_top.getPulses() * 2 * PI * WHEEL_RAD)/0.01;
00109     left_back_vel   = (enc_left_back.getPulses() * 2 * PI * WHEEL_RAD)/0.01;
00110     right_back_vel  = (enc_right_back.getPulses() * 2 * PI * WHEEL_RAD)/0.01;
00111 
00112     //compute PID
00113     float left_top_target_rate = locomotion_left_top.createpwm(wheels_target_velocity[0], 
00114                                                               left_top_vel);
00115     float right_top_target_rate = locomotion_right_top.createpwm(wheels_target_velocity[1], 
00116                                                               right_top_vel);
00117     float left_back_target_rate = locomotion_left_back.createpwm(wheels_target_velocity[2], 
00118                                                               left_back_vel);
00119     float right_back_target_rate = locomotion_right_back.createpwm(wheels_target_velocity[3], 
00120                                                               right_back_vel);
00121     //rate to motor
00122     motor_left_top.speed(left_top_target_rate);
00123     motor_right_top.speed(right_top_target_rate);
00124     motor_left_back.speed(left_back_target_rate);
00125     motor_right_back.speed(right_back_target_rate);
00126 
00127     enc_left_back.reset();
00128     enc_right_back.reset();
00129     enc_left_top.reset();
00130     enc_right_top.reset();
00131 }
00132 
00133 void shagaiMovement(int _state, float _theta, bool _up){
00134     if ((_state == 5) && (!(_up))){
00135         if(_theta<30){
00136             motor_shagai.speed(0.5);
00137         }else if(_theta<100){
00138             motor_shagai.speed(0.3);
00139         }else if((_theta>220) && (_theta<250)){
00140             motor_shagai.speed(-0.3);
00141         }
00142         else{
00143             motor_shagai.brake(1);
00144         }
00145     }else if ((_state == 8)&&(_up)){
00146         if(fabs(_theta)<150){
00147             motor_shagai.speed(-0.8);
00148         }else if(fabs(_theta)<210){
00149             motor_shagai.speed(-0.5);
00150         }
00151         else{
00152             motor_shagai.brake(1);
00153         }
00154     }else{
00155         motor_shagai.brake(1);
00156     } 
00157 }
00158 
00159 void shagaiParameter(){
00160     float shagai_pulse = enc_shagai.getPulses()*360.0;
00161     theta_shagai += shagai_pulse;
00162     enc_shagai.reset();
00163 }
00164 
00165 void pneumaticMovement(){
00166     //state
00167     if (state_condition<0)
00168         state_condition=0;
00169     if (state_condition>15)
00170         state_condition=5;
00171     //State 0: Kondisi awal
00172     if (state_condition == 0){
00173         pneu[0]=1;      //Pneu pengambil shagai
00174         pneu[1]=1;      //Pneu untuk ekstension pelotar
00175         pneu[2]=1;      //Pneu untuk pelontar
00176         pneu[3]=0;      //Pneu untuk pencapit gerege
00177         pneu[4]=1;      //Pneu untuk penaik gerege
00178     }
00179     //state 1: kondisi pencapit tertutup
00180     if (state_condition == 1){
00181         pneu[0]=1;      //Pneu pengambil shagai
00182         pneu[1]=1;      //Pneu untuk ekstension pelotar
00183         pneu[2]=1;      //Pneu untuk pelontar
00184         pneu[3]=1;      //Pneu untuk pencapit gerege
00185         pneu[4]=1;      //Pneu untuk penaik gerege
00186     }
00187     //state 2: tangan gerege turun untuk persiapan menyerahkan gerege ke kuda
00188     if (state_condition == 2){
00189         pneu[0]=1;      //Pneu pengambil shagai
00190         pneu[1]=1;      //Pneu untuk ekstension pelotar
00191         pneu[2]=1;      //Pneu untuk pelontar
00192         pneu[3]=1;      //Pneu untuk pencapit gerege
00193         pneu[4]=0;      //Pneu untuk penaik gerege
00194     }
00195     //state 3: penjepit gerege terbuka,gerege akan jatuh ke kuda
00196     if (state_condition == 3){
00197         pneu[0]=1;      //Pneu pengambil shagai
00198         pneu[1]=1;      //Pneu untuk ekstension pelotar
00199         pneu[2]=1;      //Pneu untuk pelontar
00200         pneu[3]=0;      //Pneu untuk pencapit gerege
00201         pneu[4]=0;      //Pneu untuk penaik gerege
00202     }
00203     //state4: penaik gerege naik ke atas
00204     if (state_condition==4){
00205         pneu[0]=1;      //Pneu pengambil shagai
00206         pneu[1]=1;      //Pneu untuk ekstension pelotar
00207         pneu[2]=1;      //Pneu untuk pelontar
00208         pneu[3]=0;      //Pneu untuk pencapit gerege
00209         pneu[4]=1;      //Pneu untuk penaik gerege
00210     }
00211     //state 5: Motor pelontar turun
00212     if(state_condition==5){
00213        up=0;
00214     }
00215     //state 6: arm terbuka untuk mengambil shagai
00216     if (state_condition == 6) {
00217         //Reset encoder motor Shagai
00218         if(count_reset_arm==0){
00219             theta_shagai=0;
00220             count_reset_arm++;
00221         }
00222         pneu[0]=0;      //Pneu pengambil shagai
00223         pneu[1]=1;      //Pneu untuk ekstension pelotar
00224         pneu[2]=1;      //Pneu untuk pelontar
00225         pneu[3]=0;      //Pneu untuk pencapit gerege
00226         pneu[4]=1;      //Pneu untuk penaik gerege
00227     }
00228     //state 7: arm tertutup untuk menjepit shagai
00229     if (state_condition == 7) {
00230         pneu[0]=1;      //Pneu pengambil shagai
00231         pneu[1]=1;      //Pneu untuk ekstension pelotar
00232         pneu[2]=1;      //Pneu untuk pelontar
00233         pneu[3]=0;      //Pneu untuk pencapit gerege
00234         pneu[4]=1;      //Pneu untuk penaik gerege
00235     }
00236     if(state_condition==8){
00237        up=1;
00238     }
00239     //state 9: pengambil shagai terbuka
00240     if (state_condition==9){
00241         //Reset encoder motor Shagai
00242         if(count_reset_arm==1){
00243             theta_shagai=0;
00244             count_reset_arm--;
00245         }
00246         pneu[0]=0;      //Pneu pengambil shagai
00247         pneu[1]=1;      //Pneu untuk ekstension pelotar
00248         pneu[2]=1;      //Pneu untuk pelontar
00249         pneu[3]=0;      //Pneu untuk pencapit gerege
00250         pneu[4]=1;      //Pneu untuk penaik gerege
00251     }
00252     //state 10: pengambil shagai tertutup
00253     if (state_condition==10){
00254         pneu[0]=1;      //Pneu pengambil shagai
00255         pneu[1]=1;      //Pneu untuk ekstension pelotar
00256         pneu[2]=1;      //Pneu untuk pelontar
00257         pneu[3]=0;      //Pneu untuk pencapit gerege
00258         pneu[4]=1;      //Pneu untuk penaik gerege
00259     }
00260     //state 11 : arm ekstension maju
00261     if (state_condition == 11) {
00262         pneu[0]=1;      //Pneu pengambil shagai
00263         pneu[1]=0;      //Pneu untuk ekstension pelotar
00264         pneu[2]=1;      //Pneu untuk pelontar
00265         pneu[3]=0;      //Pneu untuk pencapit gerege
00266         pneu[4]=1;      //Pneu untuk penaik gerege
00267     }
00268     //state 12 : arm terbuka, persiapan menembak
00269     if (state_condition == 12){
00270         pneu[0]=0;      //Pneu pengambil shagai
00271         pneu[1]=0;      //Pneu untuk ekstension pelotar
00272         pneu[2]=1;      //Pneu untuk pelontar
00273         pneu[3]=0;      //Pneu untuk pencapit gerege
00274         pneu[4]=1;      //Pneu untuk penaik gerege
00275     }
00276     //state 13 : shagai ditembak ke depan
00277     if (state_condition == 13) {
00278         pneu[0]=0;      //Pneu pengambil shagai
00279         pneu[1]=0;      //Pneu untuk ekstension pelotar
00280         pneu[2]=0;      //Pneu untuk pelontar
00281         pneu[3]=0;      //Pneu untuk pencapit gerege
00282         pneu[4]=1;      //Pneu untuk penaik gerege
00283     }
00284     //state 14: penjepit shagai tertutup dan pelontarnya memendek
00285     if (state_condition==14){
00286         pneu[0]=1;      //Pneu pengambil shagai
00287         pneu[1]=1;      //Pneu untuk ekstension pelotar
00288         pneu[2]=1;      //Pneu untuk pelontar
00289         pneu[3]=0;      //Pneu untuk pencapit gerege
00290         pneu[4]=1;      //Pneu untuk penaik gerege
00291     }
00292     //state 15 kembali ke state 5
00293     if (state_condition==15){
00294         //motor turun
00295         up=0;
00296     }
00297 }
00298 
00299 void stickState(){
00300     
00301     // RESET STATE //
00302     if (stick.START) {} 
00303     else if (stick.SELECT){}
00304 
00305     // STATE-STATE PENUMATIC //
00306     if ((millis()-prev_stick_symbol_timer>700)&&(stick.segitiga)){
00307         state_condition++;
00308         prev_stick_symbol_timer = millis();
00309     } else if ((millis()-prev_stick_symbol_timer>700)&&(stick.kotak)){
00310         state_condition--;
00311         prev_stick_symbol_timer = millis();
00312     } else if((millis()-prev_stick_symbol_timer>700)&&(stick.lingkaran)){
00313         state_condition=0;
00314         prev_stick_symbol_timer = millis();
00315     }
00316 
00317 
00318     // STICK ROTATION STATE //
00319     if (stick.L1 && !stick.R1){     
00320         linear_velocity = 0;
00321         rotation_velocity = 6.0;
00322         manual_alpha = 0;
00323         wheels_target_velocity = base._wheels_from_base(linear_velocity, rotation_velocity, manual_alpha);
00324         theta_target = compass.compass_value();
00325     } 
00326     else if (stick.R1 && !stick.L1){
00327         linear_velocity = 0;
00328         rotation_velocity = -6.0;
00329         manual_alpha = 0;
00330         wheels_target_velocity = base._wheels_from_base(linear_velocity, rotation_velocity, manual_alpha);
00331         theta_target = compass.compass_value();
00332     } 
00333 
00334 
00335     // STICK ARROW STATE //
00336     if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)
00337             &&(!stick.R2)  && (!stick.L2)&&(!stick.R1) && (!stick.L1)) {
00338         //no input condition
00339         linear_velocity = 0;
00340         manual_alpha = 0;
00341         wheels_target_velocity = base.manual_movement(robot_geometry[2],
00342                                                     theta_target,
00343                                                     linear_velocity,
00344                                                     manual_alpha);       
00345     } 
00346     else if ((!stick.lingkaran)&&(!stick.segitiga)&&(!stick.silang)
00347             &&(stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(!stick.R2)){
00348         //stick up
00349         linear_velocity = manual_linear_velocity;
00350         manual_alpha = PI/2;
00351         wheels_target_velocity = base.manual_movement(robot_geometry[2],
00352                                                     theta_target,
00353                                                     linear_velocity,
00354                                                     manual_alpha);
00355     } 
00356     else if ((!stick.lingkaran)&&(!stick.segitiga)&&(!stick.silang)
00357             &&(!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(!stick.R2)){
00358         //stick down
00359         linear_velocity = manual_linear_velocity;
00360         manual_alpha = -PI/2;
00361         wheels_target_velocity = base.manual_movement(robot_geometry[2],
00362                                                     theta_target,
00363                                                     linear_velocity,
00364                                                     manual_alpha);
00365     } 
00366     else if ( ((!stick.lingkaran)&&(!stick.segitiga)&&(!stick.silang)
00367             &&(!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri)&&(!stick.R2))){
00368         //stick right
00369         linear_velocity = manual_linear_velocity;
00370         manual_alpha = 0;
00371         wheels_target_velocity = base.manual_movement(robot_geometry[2],
00372                                                     theta_target,
00373                                                     linear_velocity,
00374                                                     manual_alpha);
00375     } 
00376     else if ((!stick.lingkaran)&&(!stick.segitiga)&&(!stick.silang)
00377             &&(!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri)&&(!stick.R2)){
00378         //stick left
00379         linear_velocity = manual_linear_velocity;
00380         manual_alpha = PI;
00381         wheels_target_velocity = base.manual_movement(robot_geometry[2],
00382                                                     theta_target,
00383                                                     linear_velocity,
00384                                                     manual_alpha);
00385     } 
00386     else if ((!stick.lingkaran)&&(!stick.segitiga)&&(!stick.silang)
00387             &&(stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri)&&(!stick.R2)){
00388         //stick right up
00389         linear_velocity = manual_linear_velocity;
00390         manual_alpha = PI/4;
00391         wheels_target_velocity = base.manual_movement(robot_geometry[2],
00392                                                     theta_target,
00393                                                     linear_velocity,
00394                                                     manual_alpha);
00395     } 
00396     else if ((!stick.lingkaran)&&(!stick.segitiga)&&(!stick.silang)
00397             &&(stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri)&&(!stick.R2)){
00398         //stick left up
00399         linear_velocity = manual_linear_velocity;
00400         manual_alpha = 3*PI/4;
00401         wheels_target_velocity = base.manual_movement(robot_geometry[2],
00402                                                     theta_target,
00403                                                     linear_velocity,
00404                                                     manual_alpha);
00405     } 
00406     else if ((!stick.lingkaran)&&(!stick.segitiga)&&(!stick.silang)
00407             &&(!stick.atas)&&(stick.bawah)&&(stick.kanan)&&(!stick.kiri)&&(!stick.R2)){ 
00408         //stick right down
00409         linear_velocity = manual_linear_velocity;
00410         manual_alpha = -3*PI/4;
00411         wheels_target_velocity = base.manual_movement(robot_geometry[2],
00412                                                     theta_target,
00413                                                     linear_velocity,
00414                                                     manual_alpha);
00415     } 
00416     else if ((!stick.lingkaran)&&(!stick.segitiga)&&(!stick.silang)
00417             &&(!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(stick.kiri)&&(!stick.R2)){
00418         //stick left down
00419         linear_velocity = manual_linear_velocity;
00420         manual_alpha = -PI/4;
00421         wheels_target_velocity = base.manual_movement(robot_geometry[2],
00422                                                     theta_target,
00423                                                     linear_velocity,
00424                                                     manual_alpha);
00425     }
00426 }