KRAI 2016 / Mbed 2 deprecated BaseHybrid_MoboV1_tesPID

base versi 1 motherboard prototype

Dependencies:   Motor NewTextLCD PID mbed

main.cpp@2:23fe981b6ef7, 2015-12-19 (annotated)

Committer:
rizqicahyo
Date:
Sat Dec 19 00:03:45 2015 +0000
Revision:
2:23fe981b6ef7
Parent:
1:33203568631b 
debug manual driving

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rizqicahyo 1:33203568631b 1 /*********************************************************************************************/
rizqicahyo 1:33203568631b 2 /** GARUDAGO-ITB (KRAI2016) **/
rizqicahyo 1:33203568631b 3 /** #ROADTOBANGKOK! **/
rizqicahyo 1:33203568631b 4 /** **/
rizqicahyo 1:33203568631b 5 /** MAIN PROGRAM ROBOT HYBRID SEMI OTOMATIS **/
rizqicahyo 1:33203568631b 6 /** **/
rizqicahyo 1:33203568631b 7 /** **/
rizqicahyo 1:33203568631b 8 /** Created by : **/
rizqicahyo 1:33203568631b 9 /** Rizqi Cahyo Yuwono **/
rizqicahyo 1:33203568631b 10 /** EL'14 - 13214090 **/
rizqicahyo 1:33203568631b 11 /** **/
rizqicahyo 1:33203568631b 12 /** Last Update : 19 Desember 2015, 06.10 WIB **/
rizqicahyo 1:33203568631b 13 /*********************************************************************************************/
rizqicahyo 1:33203568631b 14
rizqicahyo 1:33203568631b 15 /*********************************************************************************************/
rizqicahyo 1:33203568631b 16 /** FILE HEADER **/
rizqicahyo 1:33203568631b 17 /*********************************************************************************************/
rizqicahyo 0:4a1823da357f 18 #include "mbed.h"
rizqicahyo 0:4a1823da357f 19 #include "Motor.h"
rizqicahyo 0:4a1823da357f 20 #include "NewTextLCD.h"
rizqicahyo 0:4a1823da357f 21 #include "PS3_USB.h"
rizqicahyo 0:4a1823da357f 22 #include "PID.h"
rizqicahyo 0:4a1823da357f 23
rizqicahyo 0:4a1823da357f 24
rizqicahyo 1:33203568631b 25 /*********************************************************************************************/
rizqicahyo 1:33203568631b 26 /** DEKLARASI INPUT OUTPUT **/
rizqicahyo 1:33203568631b 27 /*********************************************************************************************/
rizqicahyo 0:4a1823da357f 28 // serial pc
rizqicahyo 0:4a1823da357f 29 Serial pc(USBTX,USBRX);
rizqicahyo 0:4a1823da357f 30
rizqicahyo 0:4a1823da357f 31 // LCD 20x4
rizqicahyo 0:4a1823da357f 32 TextLCD lcd(PB_13, PB_14, PB_15, PB_1, PB_2, PB_5, TextLCD::LCD20x4); //rs,e,d4-d7
rizqicahyo 0:4a1823da357f 33
rizqicahyo 0:4a1823da357f 34 // joystick PS3
rizqicahyo 1:33203568631b 35 PS3_USB PS3 (PA_11,PA_12); //(rx,tx)
rizqicahyo 0:4a1823da357f 36
rizqicahyo 1:33203568631b 37 // PID sensor garis
rizqicahyo 1:33203568631b 38 PID PID(0.532,0.000,0.41,0.001); //(P,I,D, time sampling)
rizqicahyo 0:4a1823da357f 39
rizqicahyo 0:4a1823da357f 40 // Motor
rizqicahyo 1:33203568631b 41 Motor motor_kiri (PA_15, PA_14, PA_13); //motor1 (pwm, fwd, rev)
rizqicahyo 1:33203568631b 42 Motor motor_kanan (PA_1, PC_14 ,PC_15); //motor2 (pwm, fwd, rev)
rizqicahyo 0:4a1823da357f 43
rizqicahyo 1:33203568631b 44 // Selektor dan Sensor
rizqicahyo 1:33203568631b 45 BusOut selektor(PB_0,PA_9,PC_2); //(selektor A, selektor B, selektor C)
rizqicahyo 1:33203568631b 46 AnalogIn sensorR (PC_0); //ADC1
rizqicahyo 1:33203568631b 47 AnalogIn sensorL(PC_3); //ADC2
rizqicahyo 0:4a1823da357f 48
rizqicahyo 1:33203568631b 49 // Multitasker
rizqicahyo 1:33203568631b 50 Ticker timer;
rizqicahyo 0:4a1823da357f 51
rizqicahyo 0:4a1823da357f 52
rizqicahyo 1:33203568631b 53 /*********************************************************************************************/
rizqicahyo 1:33203568631b 54 /** DEKLARASI VARIABEL GLOBAL **/
rizqicahyo 1:33203568631b 55 /*********************************************************************************************/
rizqicahyo 1:33203568631b 56 float gLimit=0.4; //nilai batas sensor agar dapat membedakan warna putih dengan background
rizqicahyo 1:33203568631b 57 float gMax_speed=0.5; //nilai maksimum kecepatan motor
rizqicahyo 1:33203568631b 58 float gMin_speed=-0.05; //nilai minimum kecepatan motor
rizqicahyo 1:33203568631b 59
rizqicahyo 1:33203568631b 60 unsigned char gMode=0; //variabel mode driving (manual = 0 otomatis = 1)
rizqicahyo 1:33203568631b 61 unsigned char gCase=0; //variabel keadaan proses
rizqicahyo 0:4a1823da357f 62
rizqicahyo 1:33203568631b 63 unsigned char logic_sensor[16]; //variabel logic dari sensor garis
rizqicahyo 1:33203568631b 64 unsigned char over=0;
rizqicahyo 1:33203568631b 65
rizqicahyo 1:33203568631b 66 unsigned char i; // variabel iterasi
rizqicahyo 1:33203568631b 67
rizqicahyo 1:33203568631b 68
rizqicahyo 1:33203568631b 69
rizqicahyo 1:33203568631b 70 /*********************************************************************************************/
rizqicahyo 1:33203568631b 71 /** DEKLARASI PROSEDUR DAN FUNGSI **/
rizqicahyo 1:33203568631b 72 /*********************************************************************************************/
rizqicahyo 1:33203568631b 73 float adcSensor(int i) //pembacaan nilai ADC dari sensor garis
rizqicahyo 0:4a1823da357f 74 {
rizqicahyo 1:33203568631b 75 if (i < 8)
rizqicahyo 0:4a1823da357f 76 {
rizqicahyo 1:33203568631b 77 selektor = 7-i;
rizqicahyo 1:33203568631b 78 return sensorL.read();
rizqicahyo 0:4a1823da357f 79 }
rizqicahyo 1:33203568631b 80 else
rizqicahyo 0:4a1823da357f 81 {
rizqicahyo 1:33203568631b 82 selektor = 15-i;
rizqicahyo 1:33203568631b 83 return sensorR.read();
rizqicahyo 0:4a1823da357f 84 }
rizqicahyo 0:4a1823da357f 85 }
rizqicahyo 0:4a1823da357f 86
rizqicahyo 1:33203568631b 87 void PIDrunning() //menjalankan perintah untuk line follower
rizqicahyo 1:33203568631b 88 {
rizqicahyo 1:33203568631b 89
rizqicahyo 1:33203568631b 90 int pv;
rizqicahyo 1:33203568631b 91 float speedR,speedL;
rizqicahyo 0:4a1823da357f 92
rizqicahyo 1:33203568631b 93 //menentukan logic sensor
rizqicahyo 1:33203568631b 94 for(i=1;i<16;i++){
rizqicahyo 1:33203568631b 95 if(adcSensor(i) <= gLimit)
rizqicahyo 1:33203568631b 96 logic_sensor[i]=1;
rizqicahyo 1:33203568631b 97 else
rizqicahyo 1:33203568631b 98 logic_sensor[i]=0;
rizqicahyo 1:33203568631b 99 }
rizqicahyo 0:4a1823da357f 100
rizqicahyo 1:33203568631b 101 //////////////////logic dari PV (present Value)/////////////////////////
rizqicahyo 0:4a1823da357f 102 if(logic_sensor[0]==1){
rizqicahyo 0:4a1823da357f 103 pv = -15;
rizqicahyo 1:33203568631b 104 over=1;
rizqicahyo 0:4a1823da357f 105 }
rizqicahyo 0:4a1823da357f 106 else if(logic_sensor[15]==1){
rizqicahyo 0:4a1823da357f 107 pv = 15;
rizqicahyo 1:33203568631b 108 over=2;
rizqicahyo 0:4a1823da357f 109 }
rizqicahyo 0:4a1823da357f 110 else if(logic_sensor[1]==1 && logic_sensor[0]==1){
rizqicahyo 0:4a1823da357f 111 pv = -14;
rizqicahyo 0:4a1823da357f 112 }
rizqicahyo 0:4a1823da357f 113 else if(logic_sensor[14]==1 && logic_sensor[15]==1){
rizqicahyo 0:4a1823da357f 114 pv = 14;
rizqicahyo 0:4a1823da357f 115 }
rizqicahyo 0:4a1823da357f 116 else if(logic_sensor[1]==1 ){
rizqicahyo 0:4a1823da357f 117 pv = -13;
rizqicahyo 0:4a1823da357f 118 }
rizqicahyo 0:4a1823da357f 119 else if(logic_sensor[14]==1 ){
rizqicahyo 0:4a1823da357f 120 pv = 13;
rizqicahyo 0:4a1823da357f 121 }
rizqicahyo 0:4a1823da357f 122 else if(logic_sensor[2]==1 && logic_sensor[1]==1){
rizqicahyo 0:4a1823da357f 123 pv = -12;
rizqicahyo 0:4a1823da357f 124 }
rizqicahyo 0:4a1823da357f 125 else if(logic_sensor[13]==1 && logic_sensor[14]==1){
rizqicahyo 0:4a1823da357f 126 pv = 12;
rizqicahyo 0:4a1823da357f 127 }
rizqicahyo 0:4a1823da357f 128 else if(logic_sensor[2]==1 ){
rizqicahyo 0:4a1823da357f 129 pv = -11;
rizqicahyo 0:4a1823da357f 130 }
rizqicahyo 0:4a1823da357f 131 else if(logic_sensor[13]==1 ){
rizqicahyo 0:4a1823da357f 132 pv = 11;
rizqicahyo 0:4a1823da357f 133 }
rizqicahyo 0:4a1823da357f 134 else if(logic_sensor[3]==1 && logic_sensor[2]==1){
rizqicahyo 0:4a1823da357f 135 pv = -10;
rizqicahyo 0:4a1823da357f 136 }
rizqicahyo 0:4a1823da357f 137 else if(logic_sensor[12]==1 && logic_sensor[13]==1){
rizqicahyo 0:4a1823da357f 138 pv = 10;
rizqicahyo 0:4a1823da357f 139 }
rizqicahyo 0:4a1823da357f 140 else if(logic_sensor[3]==1 ){
rizqicahyo 0:4a1823da357f 141 pv = -9;
rizqicahyo 0:4a1823da357f 142 }
rizqicahyo 0:4a1823da357f 143 else if(logic_sensor[12]==1 ){
rizqicahyo 0:4a1823da357f 144 pv = 9;
rizqicahyo 0:4a1823da357f 145 }
rizqicahyo 0:4a1823da357f 146 else if(logic_sensor[4]==1 && logic_sensor[3]==1){
rizqicahyo 0:4a1823da357f 147 pv = -8;
rizqicahyo 0:4a1823da357f 148 }
rizqicahyo 0:4a1823da357f 149 else if(logic_sensor[11]==1 && logic_sensor[12]==1){
rizqicahyo 0:4a1823da357f 150 pv = 8;
rizqicahyo 0:4a1823da357f 151 }
rizqicahyo 0:4a1823da357f 152 else if(logic_sensor[4]==1 ){
rizqicahyo 0:4a1823da357f 153 pv = -7;
rizqicahyo 0:4a1823da357f 154 }
rizqicahyo 0:4a1823da357f 155 else if(logic_sensor[11]==1 ){
rizqicahyo 0:4a1823da357f 156 pv = 7;
rizqicahyo 0:4a1823da357f 157 }
rizqicahyo 0:4a1823da357f 158 else if(logic_sensor[5]==1 && logic_sensor[4]==1){
rizqicahyo 0:4a1823da357f 159 pv = -6;
rizqicahyo 0:4a1823da357f 160 }
rizqicahyo 0:4a1823da357f 161 else if(logic_sensor[10]==1 && logic_sensor[11]==1){
rizqicahyo 0:4a1823da357f 162 pv = 6;
rizqicahyo 0:4a1823da357f 163 }
rizqicahyo 0:4a1823da357f 164 else if(logic_sensor[5]==1 ){
rizqicahyo 0:4a1823da357f 165 pv = -5;
rizqicahyo 0:4a1823da357f 166 }
rizqicahyo 0:4a1823da357f 167 else if(logic_sensor[10]==1 ){
rizqicahyo 0:4a1823da357f 168 pv = 5;
rizqicahyo 0:4a1823da357f 169 }
rizqicahyo 0:4a1823da357f 170 else if(logic_sensor[6]==1 && logic_sensor[5]==1){
rizqicahyo 0:4a1823da357f 171 pv = -4;
rizqicahyo 0:4a1823da357f 172 }
rizqicahyo 0:4a1823da357f 173 else if(logic_sensor[9]==1 && logic_sensor[10]==1){
rizqicahyo 0:4a1823da357f 174 pv = 4;
rizqicahyo 0:4a1823da357f 175 }
rizqicahyo 0:4a1823da357f 176 else if(logic_sensor[6]==1 ){
rizqicahyo 0:4a1823da357f 177 pv = -3;
rizqicahyo 0:4a1823da357f 178 }
rizqicahyo 0:4a1823da357f 179 else if(logic_sensor[9]==1 ){
rizqicahyo 0:4a1823da357f 180 pv = 3;
rizqicahyo 0:4a1823da357f 181 }
rizqicahyo 0:4a1823da357f 182 else if(logic_sensor[7]==1 && logic_sensor[6]==1){
rizqicahyo 0:4a1823da357f 183 pv = -2;
rizqicahyo 0:4a1823da357f 184 }
rizqicahyo 0:4a1823da357f 185 else if(logic_sensor[8]==1 && logic_sensor[9]==1){
rizqicahyo 0:4a1823da357f 186 pv = 2;
rizqicahyo 0:4a1823da357f 187 }
rizqicahyo 0:4a1823da357f 188 else if (logic_sensor[7]==1 && logic_sensor[8]==1){
rizqicahyo 0:4a1823da357f 189 pv = 0;
rizqicahyo 0:4a1823da357f 190 }
rizqicahyo 0:4a1823da357f 191 else if(logic_sensor[7]==1 ){
rizqicahyo 0:4a1823da357f 192 pv = -1;
rizqicahyo 0:4a1823da357f 193 }
rizqicahyo 0:4a1823da357f 194 else if(logic_sensor[8]==1 ){
rizqicahyo 0:4a1823da357f 195 pv = 1;
rizqicahyo 1:33203568631b 196 }
rizqicahyo 1:33203568631b 197 ///////////////// robot bergerak keluar dari sensor/////////////////////
rizqicahyo 1:33203568631b 198 if(over=1){
rizqicahyo 0:4a1823da357f 199 /*if(speed_ka > 0){
rizqicahyo 0:4a1823da357f 200 wait_ms(10);
rizqicahyo 0:4a1823da357f 201 motor_kanan.speed(-speed_ka);
rizqicahyo 0:4a1823da357f 202 wait_ms(10);
rizqicahyo 0:4a1823da357f 203 }
rizqicahyo 0:4a1823da357f 204 else{
rizqicahyo 0:4a1823da357f 205 motor_kanan.speed(speed_ka);
rizqicahyo 0:4a1823da357f 206 }
rizqicahyo 0:4a1823da357f 207 wait_ms(10);*/
rizqicahyo 0:4a1823da357f 208
rizqicahyo 1:33203568631b 209 motor_kiri.brake(1);
rizqicahyo 1:33203568631b 210 //wait_ms(100);
rizqicahyo 0:4a1823da357f 211
rizqicahyo 0:4a1823da357f 212 }
rizqicahyo 1:33203568631b 213 else if(over==2){
rizqicahyo 0:4a1823da357f 214 /*if(speed_ki > 0){
rizqicahyo 0:4a1823da357f 215 wait_ms(10);
rizqicahyo 0:4a1823da357f 216 motor_kiri.speed(-speed_ki);
rizqicahyo 0:4a1823da357f 217 wait_ms(10);
rizqicahyo 0:4a1823da357f 218 }
rizqicahyo 0:4a1823da357f 219 else{
rizqicahyo 0:4a1823da357f 220 wait_ms(10);
rizqicahyo 0:4a1823da357f 221 motor_kiri.speed(speed_ki);
rizqicahyo 0:4a1823da357f 222 wait_ms(10);
rizqicahyo 0:4a1823da357f 223 }
rizqicahyo 0:4a1823da357f 224 wait_ms(10);*/
rizqicahyo 1:33203568631b 225 motor_kanan.brake(1);
rizqicahyo 1:33203568631b 226 //wait_ms(100);
rizqicahyo 0:4a1823da357f 227 }
rizqicahyo 0:4a1823da357f 228
rizqicahyo 0:4a1823da357f 229 PID.setProcessValue(pv);
rizqicahyo 0:4a1823da357f 230 PID.setSetPoint(0);
rizqicahyo 0:4a1823da357f 231
rizqicahyo 0:4a1823da357f 232 // memulai perhitungan PID
rizqicahyo 0:4a1823da357f 233
rizqicahyo 1:33203568631b 234 speedR = gMax_speed + PID.compute();
rizqicahyo 1:33203568631b 235 if(speedR > gMax_speed){
rizqicahyo 1:33203568631b 236 speedR = gMax_speed;
rizqicahyo 0:4a1823da357f 237 }
rizqicahyo 1:33203568631b 238 else if(speedR < gMin_speed)
rizqicahyo 1:33203568631b 239 speedR = gMin_speed;
rizqicahyo 1:33203568631b 240 motor_kanan.speed(speedR);
rizqicahyo 0:4a1823da357f 241
rizqicahyo 1:33203568631b 242 speedL = gMax_speed - PID.compute();
rizqicahyo 1:33203568631b 243 if(speedL > gMax_speed)
rizqicahyo 1:33203568631b 244 speedL = gMax_speed;
rizqicahyo 1:33203568631b 245 else if(speedL < gMin_speed)
rizqicahyo 1:33203568631b 246 speedL = gMin_speed;
rizqicahyo 1:33203568631b 247 motor_kiri.speed(speedL);
rizqicahyo 1:33203568631b 248
rizqicahyo 1:33203568631b 249 over=0;
rizqicahyo 0:4a1823da357f 250 }
rizqicahyo 0:4a1823da357f 251
rizqicahyo 1:33203568631b 252 void showLCD() //menampilkan user interface pada LCD
rizqicahyo 1:33203568631b 253 {
rizqicahyo 1:33203568631b 254 lcd.cls();
rizqicahyo 1:33203568631b 255 lcd.locate(5,0);
rizqicahyo 1:33203568631b 256 lcd.printf("GarudaGo !!");
rizqicahyo 1:33203568631b 257
rizqicahyo 1:33203568631b 258 switch(gCase)
rizqicahyo 0:4a1823da357f 259 {
rizqicahyo 1:33203568631b 260 case 0 :
rizqicahyo 1:33203568631b 261 {
rizqicahyo 1:33203568631b 262 lcd.locate(1,1);
rizqicahyo 1:33203568631b 263 lcd.printf("Kalibrasi = %.4f",gLimit);
rizqicahyo 1:33203568631b 264 break;
rizqicahyo 1:33203568631b 265 }
rizqicahyo 1:33203568631b 266 case 1 :
rizqicahyo 1:33203568631b 267 {
rizqicahyo 1:33203568631b 268 if (gMode == 1)
rizqicahyo 1:33203568631b 269 {
rizqicahyo 1:33203568631b 270 lcd.locate(3,1);
rizqicahyo 1:33203568631b 271 lcd.printf("Mode = Otomatis");
rizqicahyo 1:33203568631b 272 }
rizqicahyo 1:33203568631b 273 else if (gMode==0)
rizqicahyo 1:33203568631b 274 {
rizqicahyo 1:33203568631b 275 lcd.locate(3,1);
rizqicahyo 1:33203568631b 276 lcd.printf("Mode = Manual");
rizqicahyo 1:33203568631b 277 }
rizqicahyo 1:33203568631b 278 break;
rizqicahyo 1:33203568631b 279 }
rizqicahyo 1:33203568631b 280 }
rizqicahyo 1:33203568631b 281
rizqicahyo 1:33203568631b 282 for(i=0;i<16;i++){
rizqicahyo 1:33203568631b 283 if(adcSensor(i) <= gLimit)
rizqicahyo 1:33203568631b 284 logic_sensor[i]=1;
rizqicahyo 1:33203568631b 285 else
rizqicahyo 1:33203568631b 286 logic_sensor[i]=0;
rizqicahyo 0:4a1823da357f 287 lcd.locate(i+2,3);
rizqicahyo 0:4a1823da357f 288 lcd.printf("%d",logic_sensor[i]);
rizqicahyo 0:4a1823da357f 289 }
rizqicahyo 0:4a1823da357f 290 }
rizqicahyo 0:4a1823da357f 291
rizqicahyo 1:33203568631b 292 void running() //prosedur utama untuk kendali robot
rizqicahyo 0:4a1823da357f 293 {
rizqicahyo 2:23fe981b6ef7 294 float speed=gMax_speed;
rizqicahyo 2:23fe981b6ef7 295 float k=1;
rizqicahyo 1:33203568631b 296
rizqicahyo 1:33203568631b 297 switch(gCase)
rizqicahyo 0:4a1823da357f 298 {
rizqicahyo 1:33203568631b 299 case 0 :
rizqicahyo 0:4a1823da357f 300 {
rizqicahyo 1:33203568631b 301 motor_kiri.brake(0);
rizqicahyo 1:33203568631b 302 motor_kanan.brake(0);
rizqicahyo 1:33203568631b 303
rizqicahyo 1:33203568631b 304 if(PS3.L2 == 0 && PS3.R2==255 && !PS3.R1 && !PS3.L1)
rizqicahyo 1:33203568631b 305 gLimit += 0.00001;
rizqicahyo 1:33203568631b 306 else if (PS3.L2 == 255 && PS3.R2==0 && !PS3.R1 && !PS3.L1)
rizqicahyo 1:33203568631b 307 gLimit -= 0.00001;
rizqicahyo 1:33203568631b 308 else if(PS3.L2 == 0 && PS3.R2==0 && PS3.R1 && !PS3.L1)
rizqicahyo 1:33203568631b 309 gLimit += 0.0005;
rizqicahyo 1:33203568631b 310 if(PS3.L2 == 0 && PS3.R2==0 && !PS3.R1 && PS3.L1)
rizqicahyo 1:33203568631b 311 gLimit -= 0.0005;
rizqicahyo 1:33203568631b 312
rizqicahyo 1:33203568631b 313 if (gLimit > 1.0)
rizqicahyo 1:33203568631b 314 gLimit = 1.0;
rizqicahyo 1:33203568631b 315 else if(gLimit < 0)
rizqicahyo 1:33203568631b 316 gLimit = 0.0;
rizqicahyo 1:33203568631b 317
rizqicahyo 1:33203568631b 318 if(PS3.START && PS3.L2 == 0 && PS3.R2 == 0 && !PS3.R1 && !PS3.L1)
rizqicahyo 1:33203568631b 319 gCase++;
rizqicahyo 1:33203568631b 320
rizqicahyo 1:33203568631b 321 break;
rizqicahyo 0:4a1823da357f 322 }
rizqicahyo 1:33203568631b 323 case 1 :
rizqicahyo 1:33203568631b 324 {
rizqicahyo 1:33203568631b 325 if (gMode == 1)
rizqicahyo 1:33203568631b 326 {
rizqicahyo 1:33203568631b 327 if (PS3.silang)
rizqicahyo 1:33203568631b 328 {
rizqicahyo 1:33203568631b 329 PIDrunning();
rizqicahyo 1:33203568631b 330 pc.printf("PID \t %f \t ",PID.compute());
rizqicahyo 1:33203568631b 331
rizqicahyo 1:33203568631b 332 for(i=0;i<16;i++)
rizqicahyo 1:33203568631b 333 {
rizqicahyo 1:33203568631b 334 pc.printf("%d ",logic_sensor[i]);
rizqicahyo 1:33203568631b 335 }
rizqicahyo 1:33203568631b 336 pc.printf("\n");
rizqicahyo 1:33203568631b 337 }
rizqicahyo 2:23fe981b6ef7 338 else if(!PS3.silang)
rizqicahyo 1:33203568631b 339 {
rizqicahyo 2:23fe981b6ef7 340 motor_kiri.brake(0);
rizqicahyo 2:23fe981b6ef7 341 motor_kanan.brake(0);
rizqicahyo 1:33203568631b 342 pc.printf("PID stop \n");
rizqicahyo 1:33203568631b 343 }
rizqicahyo 1:33203568631b 344 }
rizqicahyo 1:33203568631b 345 else if (gMode == 0)
rizqicahyo 1:33203568631b 346 {
rizqicahyo 1:33203568631b 347 if (PS3.atas && !PS3.bawah && !PS3.L1 && !PS3.R1) //maju
rizqicahyo 1:33203568631b 348 {
rizqicahyo 1:33203568631b 349 if (speed < 0.1)
rizqicahyo 1:33203568631b 350 speed = 0.1;
rizqicahyo 1:33203568631b 351 else
rizqicahyo 1:33203568631b 352 speed += 0.005;
rizqicahyo 1:33203568631b 353
rizqicahyo 1:33203568631b 354 if (speed >= gMax_speed)
rizqicahyo 1:33203568631b 355 speed = gMax_speed;
rizqicahyo 1:33203568631b 356
rizqicahyo 1:33203568631b 357 motor_kiri.speed(speed*k);
rizqicahyo 1:33203568631b 358 motor_kanan.speed(speed*k);
rizqicahyo 1:33203568631b 359 pc.printf("maju \n");
rizqicahyo 1:33203568631b 360 }
rizqicahyo 1:33203568631b 361 else if (!PS3.atas && PS3.bawah && !PS3.L1 && !PS3.R1) //mundur
rizqicahyo 1:33203568631b 362 {
rizqicahyo 1:33203568631b 363 if (speed < 0.1)
rizqicahyo 1:33203568631b 364 speed = 0.1;
rizqicahyo 1:33203568631b 365 else
rizqicahyo 1:33203568631b 366 speed += 0.005;
rizqicahyo 1:33203568631b 367
rizqicahyo 1:33203568631b 368 if (speed >= gMax_speed)
rizqicahyo 1:33203568631b 369 speed = gMax_speed;
rizqicahyo 1:33203568631b 370
rizqicahyo 1:33203568631b 371 motor_kiri.speed(-speed*k);
rizqicahyo 1:33203568631b 372 motor_kanan.speed(-speed*k);
rizqicahyo 1:33203568631b 373 pc.printf("mundur \n");
rizqicahyo 1:33203568631b 374 }
rizqicahyo 1:33203568631b 375 else if (!PS3.atas && !PS3.bawah && PS3.L1 && !PS3.R1) //pivot kiri
rizqicahyo 1:33203568631b 376 {
rizqicahyo 1:33203568631b 377 if (speed < 0.1)
rizqicahyo 1:33203568631b 378 speed = 0.1;
rizqicahyo 1:33203568631b 379 else
rizqicahyo 1:33203568631b 380 speed += 0.005;
rizqicahyo 1:33203568631b 381
rizqicahyo 1:33203568631b 382 if (speed >= gMax_speed)
rizqicahyo 1:33203568631b 383 speed = gMax_speed;
rizqicahyo 1:33203568631b 384
rizqicahyo 1:33203568631b 385 motor_kiri.speed(-speed*k*0.7);
rizqicahyo 1:33203568631b 386 motor_kanan.speed(speed*k*0.7);
rizqicahyo 1:33203568631b 387 pc.printf("piv kiri \n");
rizqicahyo 1:33203568631b 388 }
rizqicahyo 1:33203568631b 389 else if (!PS3.atas && !PS3.bawah && !PS3.L1 && PS3.R1) //pivot kanan
rizqicahyo 1:33203568631b 390 {
rizqicahyo 1:33203568631b 391 if (speed < 0.1)
rizqicahyo 1:33203568631b 392 speed = 0.1;
rizqicahyo 1:33203568631b 393 else
rizqicahyo 1:33203568631b 394 speed += 0.005;
rizqicahyo 1:33203568631b 395
rizqicahyo 1:33203568631b 396 if (speed >= gMax_speed)
rizqicahyo 1:33203568631b 397 speed = gMax_speed;
rizqicahyo 1:33203568631b 398
rizqicahyo 1:33203568631b 399 motor_kiri.speed(speed*k*0.7);
rizqicahyo 1:33203568631b 400 motor_kanan.speed(-speed*k*0.7);
rizqicahyo 1:33203568631b 401 pc.printf("piv kanan \n");
rizqicahyo 1:33203568631b 402 }
rizqicahyo 2:23fe981b6ef7 403 else if (!PS3.atas && !PS3.bawah && !PS3.L1 && !PS3.R1)
rizqicahyo 1:33203568631b 404 {
rizqicahyo 2:23fe981b6ef7 405 motor_kiri.brake(0);
rizqicahyo 2:23fe981b6ef7 406 motor_kanan.brake(0);
rizqicahyo 1:33203568631b 407 pc.printf("stop \n");
rizqicahyo 1:33203568631b 408 }
rizqicahyo 1:33203568631b 409
rizqicahyo 2:23fe981b6ef7 410 /*if(PS3.L2 == 255 && PS3.R2 == 0)
rizqicahyo 1:33203568631b 411 k=0.5;
rizqicahyo 1:33203568631b 412 else if (PS3.L2 == 0 && PS3.R2 == 255)
rizqicahyo 1:33203568631b 413 k=2;
rizqicahyo 1:33203568631b 414 else
rizqicahyo 2:23fe981b6ef7 415 k=1;*/
rizqicahyo 1:33203568631b 416 }
rizqicahyo 1:33203568631b 417 else
rizqicahyo 1:33203568631b 418 {
rizqicahyo 1:33203568631b 419 motor_kiri.brake(1);
rizqicahyo 1:33203568631b 420 motor_kanan.brake(1);
rizqicahyo 1:33203568631b 421 pc.printf("no PID no MANUAL \n");
rizqicahyo 1:33203568631b 422 }
rizqicahyo 1:33203568631b 423
rizqicahyo 1:33203568631b 424 if(PS3.SELECT && !PS3.START)
rizqicahyo 1:33203568631b 425 {
rizqicahyo 1:33203568631b 426 if (gMode==1)
rizqicahyo 1:33203568631b 427 gMode=0;
rizqicahyo 1:33203568631b 428 else
rizqicahyo 1:33203568631b 429 gMode=1;
rizqicahyo 1:33203568631b 430 }
rizqicahyo 1:33203568631b 431
rizqicahyo 1:33203568631b 432 if(PS3.START && !PS3.SELECT)
rizqicahyo 1:33203568631b 433 gCase--;
rizqicahyo 1:33203568631b 434
rizqicahyo 1:33203568631b 435 break;
rizqicahyo 1:33203568631b 436 }
rizqicahyo 0:4a1823da357f 437 }
rizqicahyo 0:4a1823da357f 438 }
rizqicahyo 0:4a1823da357f 439
rizqicahyo 1:33203568631b 440
rizqicahyo 1:33203568631b 441 /*********************************************************************************************/
rizqicahyo 1:33203568631b 442 /*********************************************************************************************/
rizqicahyo 1:33203568631b 443 /** PROGRAM UTAMA **/
rizqicahyo 1:33203568631b 444 /*********************************************************************************************/
rizqicahyo 1:33203568631b 445 /*********************************************************************************************/
rizqicahyo 0:4a1823da357f 446 int main(void){
rizqicahyo 1:33203568631b 447 //inisialisasi joystick
rizqicahyo 0:4a1823da357f 448 PS3.setup();
rizqicahyo 1:33203568631b 449
rizqicahyo 1:33203568631b 450 //inisialisasi PID
rizqicahyo 0:4a1823da357f 451 PID.setInputLimits(-15,15);
rizqicahyo 0:4a1823da357f 452 PID.setOutputLimits(-1.0,1.0);
rizqicahyo 0:4a1823da357f 453 PID.setMode(1.0);
rizqicahyo 0:4a1823da357f 454 PID.setBias(0.0);
rizqicahyo 0:4a1823da357f 455 PID.reset();
rizqicahyo 1:33203568631b 456
rizqicahyo 0:4a1823da357f 457 // serial PC
rizqicahyo 0:4a1823da357f 458 pc.baud(115200);
rizqicahyo 1:33203568631b 459
rizqicahyo 1:33203568631b 460 //multitasking untuk menampilkan layar
rizqicahyo 2:23fe981b6ef7 461 timer.attach(&showLCD,1);
rizqicahyo 1:33203568631b 462
rizqicahyo 1:33203568631b 463 while(1){
rizqicahyo 1:33203568631b 464 PS3.idle();
rizqicahyo 0:4a1823da357f 465
rizqicahyo 0:4a1823da357f 466 if(PS3.readable())
rizqicahyo 0:4a1823da357f 467 {
rizqicahyo 0:4a1823da357f 468 // Panggil fungsi pembacaan joystik
rizqicahyo 0:4a1823da357f 469 PS3.baca_data();
rizqicahyo 0:4a1823da357f 470 // Panggil fungsi pengolahan data joystik
rizqicahyo 0:4a1823da357f 471 PS3.olah_data();
rizqicahyo 0:4a1823da357f 472 //pc.printf("%2x %2x %2x %2x %3d %3d %3d %3d %3d %3d\n\r",PS3.button, PS3.RL, PS3.button_click, PS3.RL_click, PS3.R2, PS3.L2, PS3.RX, PS3.RY, PS3.LX, PS3.LY);
rizqicahyo 1:33203568631b 473 running();
rizqicahyo 0:4a1823da357f 474 }
rizqicahyo 0:4a1823da357f 475 else
rizqicahyo 0:4a1823da357f 476 {
rizqicahyo 0:4a1823da357f 477 PS3.idle();
rizqicahyo 0:4a1823da357f 478 }
rizqicahyo 0:4a1823da357f 479 }
rizqicahyo 0:4a1823da357f 480 }