KRAI 2017 / Mbed 2 deprecated Base_Hybrid_Latihan_Ok_Hajar_servo_senso

progam hybrid. versi lawas

Dependencies:   ESC Motor NewTextLCD PID PS3 PS_PAD Ping Servo mbed millis

Fork of Base_Hybrid_Latihan_Ok_Hajar_servo_sensor by KRAI 2016

main.cpp@1:c9f11055fb12, 2016-02-15 (annotated)

Committer:
rizqicahyo
Date:
Mon Feb 15 17:36:33 2016 +0000
Revision:
1:c9f11055fb12
Parent:
0:ac7353383a8e
Child:
2:2f7bed7fb055
update tuning motor;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rizqicahyo 0:ac7353383a8e 1 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 2 /** GARUDAGO-ITB (KRAI2016) **/
rizqicahyo 0:ac7353383a8e 3 /** #ROADTOBANGKOK! **/
rizqicahyo 0:ac7353383a8e 4 /** **/
rizqicahyo 0:ac7353383a8e 5 /** MAIN PROGRAM ROBOT HYBRID SEMI OTOMATIS **/
rizqicahyo 0:ac7353383a8e 6 /** **/
rizqicahyo 0:ac7353383a8e 7 /** **/
rizqicahyo 0:ac7353383a8e 8 /** Created by : **/
rizqicahyo 0:ac7353383a8e 9 /** Rizqi Cahyo Yuwono **/
rizqicahyo 0:ac7353383a8e 10 /** EL'14 - 13214090 **/
rizqicahyo 0:ac7353383a8e 11 /** **/
rizqicahyo 0:ac7353383a8e 12 /** Last Update : 19 Desember 2015, 06.10 WIB **/
rizqicahyo 0:ac7353383a8e 13 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 14
rizqicahyo 0:ac7353383a8e 15 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 16 /** FILE HEADER **/
rizqicahyo 0:ac7353383a8e 17 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 18 #include "mbed.h"
rizqicahyo 0:ac7353383a8e 19 #include "Motor.h"
rizqicahyo 0:ac7353383a8e 20 #include "NewTextLCD.h"
rizqicahyo 0:ac7353383a8e 21 #include "PS_PAD.h"
rizqicahyo 0:ac7353383a8e 22 #include "PID.h"
rizqicahyo 1:c9f11055fb12 23 #include "millis.h"
rizqicahyo 0:ac7353383a8e 24
rizqicahyo 0:ac7353383a8e 25 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 26 /** DEKLARASI INPUT OUTPUT **/
rizqicahyo 0:ac7353383a8e 27 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 28 // serial pc
rizqicahyo 0:ac7353383a8e 29 Serial pc(USBTX,USBRX);
rizqicahyo 0:ac7353383a8e 30
rizqicahyo 0:ac7353383a8e 31 // LCD 20x4
rizqicahyo 0:ac7353383a8e 32 TextLCD lcd(PC_5, PB_1, PA_7, PC_4, PA_5, PA_6, TextLCD::LCD20x4); //(rs,e,d4,d5,d6,d7)
rizqicahyo 0:ac7353383a8e 33
rizqicahyo 0:ac7353383a8e 34 // joystick PS2
rizqicahyo 0:ac7353383a8e 35 PS_PAD ps2(PB_15,PB_14,PB_13, PB_12); //(mosi, miso, sck, ss)
rizqicahyo 0:ac7353383a8e 36
rizqicahyo 0:ac7353383a8e 37 // PID sensor garis
rizqicahyo 1:c9f11055fb12 38 PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling)
rizqicahyo 0:ac7353383a8e 39
rizqicahyo 0:ac7353383a8e 40 // Motor(pwm, fwd, rev)
rizqicahyo 0:ac7353383a8e 41 Motor gripper(PB_6, PB_8, PB_9);
rizqicahyo 0:ac7353383a8e 42 Motor motor3(PC_6, PC_8, PC_9);
rizqicahyo 0:ac7353383a8e 43 Motor motor2(PB_3, PB_5, PB_4); //kanan
rizqicahyo 0:ac7353383a8e 44 Motor motor1(PA_8, PA_9, PC_7); //kiri
rizqicahyo 0:ac7353383a8e 45
rizqicahyo 0:ac7353383a8e 46 // Sensor
rizqicahyo 0:ac7353383a8e 47 DigitalIn S1(PC_0);
rizqicahyo 0:ac7353383a8e 48 DigitalIn S2(PC_1);
rizqicahyo 0:ac7353383a8e 49 DigitalIn S3(PC_2);
rizqicahyo 0:ac7353383a8e 50 DigitalIn S4(PC_3);
rizqicahyo 0:ac7353383a8e 51 DigitalIn S5(PA_0);
rizqicahyo 0:ac7353383a8e 52 DigitalIn S6(PA_1);
rizqicahyo 0:ac7353383a8e 53 DigitalIn S7(PA_4);
rizqicahyo 0:ac7353383a8e 54 DigitalIn S8(PB_0);
rizqicahyo 0:ac7353383a8e 55 DigitalIn S9(PB_2);
rizqicahyo 0:ac7353383a8e 56 DigitalIn S10(PB_10);
rizqicahyo 0:ac7353383a8e 57 DigitalIn S11(PA_10);
rizqicahyo 0:ac7353383a8e 58 DigitalIn S12(PA_11);
rizqicahyo 0:ac7353383a8e 59 DigitalIn S13(PA_12);
rizqicahyo 0:ac7353383a8e 60 DigitalOut calibrate(PA_15);
rizqicahyo 0:ac7353383a8e 61
rizqicahyo 0:ac7353383a8e 62 DigitalIn button(USER_BUTTON);
rizqicahyo 0:ac7353383a8e 63
rizqicahyo 0:ac7353383a8e 64 bool sensor[13];
rizqicahyo 0:ac7353383a8e 65
rizqicahyo 0:ac7353383a8e 66 //DigitalIn limit_switch1(A0);
rizqicahyo 0:ac7353383a8e 67 //DigitalIn limit_switch2(A1);
rizqicahyo 0:ac7353383a8e 68
rizqicahyo 0:ac7353383a8e 69
rizqicahyo 0:ac7353383a8e 70 // Multitasker
rizqicahyo 0:ac7353383a8e 71 Ticker timer;
rizqicahyo 0:ac7353383a8e 72
rizqicahyo 0:ac7353383a8e 73
rizqicahyo 0:ac7353383a8e 74 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 75 /** DEKLARASI VARIABEL GLOBAL **/
rizqicahyo 0:ac7353383a8e 76 /*********************************************************************************************/
rizqicahyo 1:c9f11055fb12 77 float gMax_speed=0.7; //nilai maksimum kecepatan motor
rizqicahyo 0:ac7353383a8e 78 float gMin_speed=-0.05; //nilai minimum kecepatan motor
rizqicahyo 1:c9f11055fb12 79 float gTuning = 0.36;
rizqicahyo 0:ac7353383a8e 80
rizqicahyo 0:ac7353383a8e 81 unsigned char gMode=0; //variabel mode driving (manual = 0 otomatis = 1)
rizqicahyo 0:ac7353383a8e 82 unsigned char gCase=0; //variabel keadaan proses
rizqicahyo 0:ac7353383a8e 83
rizqicahyo 0:ac7353383a8e 84 unsigned char i; // variabel iterasi
rizqicahyo 1:c9f11055fb12 85 int over=0;
rizqicahyo 0:ac7353383a8e 86
rizqicahyo 0:ac7353383a8e 87
rizqicahyo 0:ac7353383a8e 88
rizqicahyo 0:ac7353383a8e 89 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 90 /** DEKLARASI PROSEDUR DAN FUNGSI **/
rizqicahyo 0:ac7353383a8e 91 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 92 void init_sensor()
rizqicahyo 0:ac7353383a8e 93 {
rizqicahyo 1:c9f11055fb12 94 if((ps2.read(PS_PAD::PAD_CIRCLE)==1))
rizqicahyo 0:ac7353383a8e 95 {
rizqicahyo 0:ac7353383a8e 96 calibrate=0;
rizqicahyo 0:ac7353383a8e 97 }
rizqicahyo 0:ac7353383a8e 98 else
rizqicahyo 0:ac7353383a8e 99 {
rizqicahyo 0:ac7353383a8e 100 calibrate=1;
rizqicahyo 0:ac7353383a8e 101 }
rizqicahyo 0:ac7353383a8e 102
rizqicahyo 0:ac7353383a8e 103 sensor[0]=(S1.read()==1);
rizqicahyo 0:ac7353383a8e 104 sensor[1]=(S2.read()==1);
rizqicahyo 0:ac7353383a8e 105 sensor[2]=(S3.read()==1);
rizqicahyo 0:ac7353383a8e 106 sensor[3]=(S4.read()==1);
rizqicahyo 0:ac7353383a8e 107 sensor[4]=(S5.read()==1);
rizqicahyo 0:ac7353383a8e 108 sensor[5]=(S6.read()==1);
rizqicahyo 0:ac7353383a8e 109 sensor[6]=(S7.read()==1);
rizqicahyo 0:ac7353383a8e 110 sensor[7]=(S8.read()==1);
rizqicahyo 0:ac7353383a8e 111 sensor[8]=(S9.read()==1);
rizqicahyo 0:ac7353383a8e 112 sensor[9]=(S10.read()==1);
rizqicahyo 0:ac7353383a8e 113 sensor[10]=(S11.read()==1);
rizqicahyo 0:ac7353383a8e 114 sensor[11]=(S12.read()==1);
rizqicahyo 0:ac7353383a8e 115 sensor[12]=(S13.read()==1);
rizqicahyo 0:ac7353383a8e 116 }
rizqicahyo 0:ac7353383a8e 117
rizqicahyo 0:ac7353383a8e 118
rizqicahyo 0:ac7353383a8e 119 void PIDrunning() //menjalankan perintah untuk line follower
rizqicahyo 0:ac7353383a8e 120 {
rizqicahyo 0:ac7353383a8e 121 int pv;
rizqicahyo 1:c9f11055fb12 122
rizqicahyo 0:ac7353383a8e 123 float speedR,speedL;
rizqicahyo 0:ac7353383a8e 124
rizqicahyo 0:ac7353383a8e 125 //init_sensor();
rizqicahyo 0:ac7353383a8e 126 //////////////////logic dari PV (present Value)/////////////////////////
rizqicahyo 0:ac7353383a8e 127 if(sensor[0]){
rizqicahyo 0:ac7353383a8e 128 pv = -12;
rizqicahyo 0:ac7353383a8e 129 over=1;
rizqicahyo 0:ac7353383a8e 130 }
rizqicahyo 0:ac7353383a8e 131 else if(sensor[12]){
rizqicahyo 0:ac7353383a8e 132 pv = 12;
rizqicahyo 0:ac7353383a8e 133 over=2;
rizqicahyo 0:ac7353383a8e 134 }
rizqicahyo 0:ac7353383a8e 135 else if(sensor[0] && sensor[1]){
rizqicahyo 0:ac7353383a8e 136 pv = -10;
rizqicahyo 0:ac7353383a8e 137 }
rizqicahyo 0:ac7353383a8e 138 else if(sensor[11] && sensor[12]){
rizqicahyo 0:ac7353383a8e 139 pv = 10;
rizqicahyo 0:ac7353383a8e 140 }
rizqicahyo 0:ac7353383a8e 141 else if(sensor[1]){
rizqicahyo 0:ac7353383a8e 142 pv = -9;
rizqicahyo 0:ac7353383a8e 143 }
rizqicahyo 0:ac7353383a8e 144 else if(sensor[11]){
rizqicahyo 0:ac7353383a8e 145 pv = 9;
rizqicahyo 0:ac7353383a8e 146 }
rizqicahyo 0:ac7353383a8e 147 else if(sensor[1] && sensor[2]){
rizqicahyo 0:ac7353383a8e 148 pv = -8;
rizqicahyo 0:ac7353383a8e 149 }
rizqicahyo 0:ac7353383a8e 150 else if(sensor[10] && sensor[11]){
rizqicahyo 0:ac7353383a8e 151 pv = 8;
rizqicahyo 0:ac7353383a8e 152 }
rizqicahyo 0:ac7353383a8e 153 else if(sensor[2]){
rizqicahyo 0:ac7353383a8e 154 pv = -7;
rizqicahyo 0:ac7353383a8e 155 }
rizqicahyo 0:ac7353383a8e 156 else if(sensor[10]){
rizqicahyo 0:ac7353383a8e 157 pv = 7;
rizqicahyo 0:ac7353383a8e 158 }
rizqicahyo 0:ac7353383a8e 159 else if(sensor[2] && sensor[3]){
rizqicahyo 0:ac7353383a8e 160 pv = -6;
rizqicahyo 0:ac7353383a8e 161 }
rizqicahyo 0:ac7353383a8e 162 else if(sensor[9] && sensor[10]){
rizqicahyo 0:ac7353383a8e 163 pv = 6;
rizqicahyo 0:ac7353383a8e 164 }
rizqicahyo 0:ac7353383a8e 165 else if(sensor[3]){
rizqicahyo 0:ac7353383a8e 166 pv = -5;
rizqicahyo 0:ac7353383a8e 167 }
rizqicahyo 0:ac7353383a8e 168 else if(sensor[9]){
rizqicahyo 0:ac7353383a8e 169 pv = 5;
rizqicahyo 0:ac7353383a8e 170 }
rizqicahyo 0:ac7353383a8e 171 else if(sensor[3] && sensor[4]){
rizqicahyo 0:ac7353383a8e 172 pv = -4;
rizqicahyo 0:ac7353383a8e 173 }
rizqicahyo 0:ac7353383a8e 174 else if(sensor[8] && sensor[9]){
rizqicahyo 0:ac7353383a8e 175 pv = 4;
rizqicahyo 0:ac7353383a8e 176 }
rizqicahyo 0:ac7353383a8e 177 else if(sensor[4]){
rizqicahyo 0:ac7353383a8e 178 pv = -3;
rizqicahyo 0:ac7353383a8e 179 }
rizqicahyo 0:ac7353383a8e 180 else if(sensor[8]){
rizqicahyo 0:ac7353383a8e 181 pv = 3;
rizqicahyo 0:ac7353383a8e 182 }
rizqicahyo 0:ac7353383a8e 183 else if(sensor[4] && sensor[5]){
rizqicahyo 0:ac7353383a8e 184 pv = -2;
rizqicahyo 0:ac7353383a8e 185 }
rizqicahyo 0:ac7353383a8e 186 else if(sensor[7] && sensor[8]){
rizqicahyo 0:ac7353383a8e 187 pv = 2;
rizqicahyo 0:ac7353383a8e 188 }
rizqicahyo 0:ac7353383a8e 189 else if(sensor[5]){
rizqicahyo 0:ac7353383a8e 190 pv = -1;
rizqicahyo 0:ac7353383a8e 191 }
rizqicahyo 0:ac7353383a8e 192 else if(sensor[7]){
rizqicahyo 0:ac7353383a8e 193 pv = 1;
rizqicahyo 0:ac7353383a8e 194 }
rizqicahyo 0:ac7353383a8e 195 else if(sensor[5] && sensor[6]){
rizqicahyo 0:ac7353383a8e 196 pv = -0.5;
rizqicahyo 0:ac7353383a8e 197 }
rizqicahyo 0:ac7353383a8e 198 else if(sensor[6] && sensor[7]){
rizqicahyo 0:ac7353383a8e 199 pv = 0.5;
rizqicahyo 0:ac7353383a8e 200 }
rizqicahyo 0:ac7353383a8e 201 else if (sensor[6]){
rizqicahyo 0:ac7353383a8e 202 pv = 0;
rizqicahyo 0:ac7353383a8e 203 }
rizqicahyo 1:c9f11055fb12 204 else
rizqicahyo 1:c9f11055fb12 205 {
rizqicahyo 1:c9f11055fb12 206 ///////////////// robot bergerak keluar dari sensor/////////////////////
rizqicahyo 0:ac7353383a8e 207 if(over==1){
rizqicahyo 0:ac7353383a8e 208 /*if(speed_ka > 0){
rizqicahyo 0:ac7353383a8e 209 wait_ms(10);
rizqicahyo 0:ac7353383a8e 210 motor2.speed(-speed_ka);
rizqicahyo 0:ac7353383a8e 211 wait_ms(10);
rizqicahyo 0:ac7353383a8e 212 }
rizqicahyo 0:ac7353383a8e 213 else{
rizqicahyo 0:ac7353383a8e 214 motor2.speed(speed_ka);
rizqicahyo 0:ac7353383a8e 215 }
rizqicahyo 0:ac7353383a8e 216 wait_ms(10);*/
rizqicahyo 0:ac7353383a8e 217
rizqicahyo 0:ac7353383a8e 218 motor1.brake(1);
rizqicahyo 0:ac7353383a8e 219 //wait_ms(100);
rizqicahyo 0:ac7353383a8e 220
rizqicahyo 0:ac7353383a8e 221 }
rizqicahyo 0:ac7353383a8e 222 else if(over==2){
rizqicahyo 0:ac7353383a8e 223 /*if(speed_ki > 0){
rizqicahyo 0:ac7353383a8e 224 wait_ms(10);
rizqicahyo 0:ac7353383a8e 225 motor1.speed(-speed_ki);
rizqicahyo 0:ac7353383a8e 226 wait_ms(10);
rizqicahyo 0:ac7353383a8e 227 }
rizqicahyo 0:ac7353383a8e 228 else{
rizqicahyo 0:ac7353383a8e 229 wait_ms(10);
rizqicahyo 0:ac7353383a8e 230 motor1.speed(speed_ki);
rizqicahyo 0:ac7353383a8e 231 wait_ms(10);
rizqicahyo 0:ac7353383a8e 232 }
rizqicahyo 0:ac7353383a8e 233 wait_ms(10);*/
rizqicahyo 0:ac7353383a8e 234 motor2.brake(1);
rizqicahyo 0:ac7353383a8e 235 //wait_ms(100);
rizqicahyo 0:ac7353383a8e 236 }
rizqicahyo 1:c9f11055fb12 237 }
rizqicahyo 0:ac7353383a8e 238 PID.setProcessValue(pv);
rizqicahyo 0:ac7353383a8e 239 PID.setSetPoint(0);
rizqicahyo 0:ac7353383a8e 240
rizqicahyo 0:ac7353383a8e 241 // memulai perhitungan PID
rizqicahyo 0:ac7353383a8e 242
rizqicahyo 0:ac7353383a8e 243 speedR = gMax_speed - PID.compute();
rizqicahyo 0:ac7353383a8e 244 if(speedR > gMax_speed){
rizqicahyo 0:ac7353383a8e 245 speedR = gMax_speed;
rizqicahyo 0:ac7353383a8e 246 }
rizqicahyo 0:ac7353383a8e 247 else if(speedR < gMin_speed)
rizqicahyo 0:ac7353383a8e 248 speedR = gMin_speed;
rizqicahyo 0:ac7353383a8e 249 motor2.speed(speedR);
rizqicahyo 0:ac7353383a8e 250
rizqicahyo 0:ac7353383a8e 251 speedL = gMax_speed + PID.compute();
rizqicahyo 0:ac7353383a8e 252 if(speedL > gMax_speed)
rizqicahyo 0:ac7353383a8e 253 speedL = gMax_speed;
rizqicahyo 0:ac7353383a8e 254 else if(speedL < gMin_speed)
rizqicahyo 0:ac7353383a8e 255 speedL = gMin_speed;
rizqicahyo 0:ac7353383a8e 256 motor1.speed(speedL);
rizqicahyo 0:ac7353383a8e 257 }
rizqicahyo 0:ac7353383a8e 258
rizqicahyo 0:ac7353383a8e 259 void showLCD() //menampilkan user interface pada LCD
rizqicahyo 0:ac7353383a8e 260 {
rizqicahyo 0:ac7353383a8e 261 lcd.cls();
rizqicahyo 0:ac7353383a8e 262 lcd.locate(5,0);
rizqicahyo 0:ac7353383a8e 263 lcd.printf("GarudaGo !!");
rizqicahyo 0:ac7353383a8e 264
rizqicahyo 0:ac7353383a8e 265 switch(gCase)
rizqicahyo 0:ac7353383a8e 266 {
rizqicahyo 0:ac7353383a8e 267 case 0 :
rizqicahyo 0:ac7353383a8e 268 {
rizqicahyo 0:ac7353383a8e 269 lcd.locate(2,2);
rizqicahyo 0:ac7353383a8e 270 lcd.printf("speed = %.4f",gMax_speed);
rizqicahyo 0:ac7353383a8e 271 break;
rizqicahyo 0:ac7353383a8e 272 }
rizqicahyo 0:ac7353383a8e 273 case 1 :
rizqicahyo 0:ac7353383a8e 274 {
rizqicahyo 0:ac7353383a8e 275 if (gMode == 1)
rizqicahyo 0:ac7353383a8e 276 {
rizqicahyo 0:ac7353383a8e 277 lcd.locate(3,1);
rizqicahyo 0:ac7353383a8e 278 lcd.printf("Mode = Otomatis");
rizqicahyo 0:ac7353383a8e 279 }
rizqicahyo 0:ac7353383a8e 280 else if (gMode==0)
rizqicahyo 0:ac7353383a8e 281 {
rizqicahyo 0:ac7353383a8e 282 lcd.locate(3,1);
rizqicahyo 0:ac7353383a8e 283 lcd.printf("Mode = Manual");
rizqicahyo 0:ac7353383a8e 284 }
rizqicahyo 0:ac7353383a8e 285 break;
rizqicahyo 0:ac7353383a8e 286 }
rizqicahyo 0:ac7353383a8e 287 }
rizqicahyo 0:ac7353383a8e 288
rizqicahyo 0:ac7353383a8e 289 lcd.locate(2,3);
rizqicahyo 0:ac7353383a8e 290 lcd.printf("%d",S1.read());
rizqicahyo 0:ac7353383a8e 291 lcd.locate(3,3);
rizqicahyo 0:ac7353383a8e 292 lcd.printf("%d",S2.read());
rizqicahyo 0:ac7353383a8e 293 lcd.locate(4,3);
rizqicahyo 0:ac7353383a8e 294 lcd.printf("%d",S3.read());
rizqicahyo 0:ac7353383a8e 295 lcd.locate(5,3);
rizqicahyo 0:ac7353383a8e 296 lcd.printf("%d",S4.read());
rizqicahyo 0:ac7353383a8e 297 lcd.locate(6,3);
rizqicahyo 0:ac7353383a8e 298 lcd.printf("%d",S5.read());
rizqicahyo 0:ac7353383a8e 299
rizqicahyo 0:ac7353383a8e 300 lcd.locate(7,3);
rizqicahyo 0:ac7353383a8e 301 lcd.printf("%d",S6.read());
rizqicahyo 0:ac7353383a8e 302 lcd.locate(8,3);
rizqicahyo 0:ac7353383a8e 303 lcd.printf("%d",S7.read());
rizqicahyo 0:ac7353383a8e 304 lcd.locate(9,3);
rizqicahyo 0:ac7353383a8e 305 lcd.printf("%d",S8.read());
rizqicahyo 0:ac7353383a8e 306 lcd.locate(10,3);
rizqicahyo 0:ac7353383a8e 307 lcd.printf("%d",S9.read());
rizqicahyo 0:ac7353383a8e 308 lcd.locate(11,3);
rizqicahyo 0:ac7353383a8e 309 lcd.printf("%d",S10.read());
rizqicahyo 0:ac7353383a8e 310
rizqicahyo 0:ac7353383a8e 311 lcd.locate(12,3);
rizqicahyo 0:ac7353383a8e 312 lcd.printf("%d",S11.read());
rizqicahyo 0:ac7353383a8e 313 lcd.locate(13,3);
rizqicahyo 0:ac7353383a8e 314 lcd.printf("%d",S12.read());
rizqicahyo 0:ac7353383a8e 315 lcd.locate(14,3);
rizqicahyo 0:ac7353383a8e 316 lcd.printf("%d",S13.read());
rizqicahyo 0:ac7353383a8e 317 }
rizqicahyo 0:ac7353383a8e 318
rizqicahyo 0:ac7353383a8e 319
rizqicahyo 0:ac7353383a8e 320 void running() //prosedur utama untuk kendali robot
rizqicahyo 0:ac7353383a8e 321 {
rizqicahyo 0:ac7353383a8e 322 float speed=gMax_speed;
rizqicahyo 0:ac7353383a8e 323 float k=1;
rizqicahyo 0:ac7353383a8e 324
rizqicahyo 0:ac7353383a8e 325 switch(gCase)
rizqicahyo 0:ac7353383a8e 326 {
rizqicahyo 0:ac7353383a8e 327 case 0 :
rizqicahyo 0:ac7353383a8e 328 {
rizqicahyo 0:ac7353383a8e 329 motor1.brake(1);
rizqicahyo 0:ac7353383a8e 330 motor2.brake(1);
rizqicahyo 0:ac7353383a8e 331
rizqicahyo 0:ac7353383a8e 332 if((ps2.read(PS_PAD::PAD_R2)==1) && (ps2.read(PS_PAD::PAD_L2)==0))
rizqicahyo 0:ac7353383a8e 333 gMax_speed += 0.00008;
rizqicahyo 0:ac7353383a8e 334 else if((ps2.read(PS_PAD::PAD_R2)==0) && (ps2.read(PS_PAD::PAD_L2)==1))
rizqicahyo 0:ac7353383a8e 335 gMax_speed -= 0.00008;
rizqicahyo 0:ac7353383a8e 336
rizqicahyo 0:ac7353383a8e 337 if (gMax_speed > 1.0)
rizqicahyo 0:ac7353383a8e 338 gMax_speed = 1.0;
rizqicahyo 0:ac7353383a8e 339 else if(gMax_speed < 0)
rizqicahyo 0:ac7353383a8e 340 gMax_speed = 0.0;
rizqicahyo 0:ac7353383a8e 341
rizqicahyo 0:ac7353383a8e 342 if((ps2.read(PS_PAD::PAD_START)==1) && (ps2.read(PS_PAD::PAD_R2)==0) && (ps2.read(PS_PAD::PAD_L2)==0))
rizqicahyo 0:ac7353383a8e 343 {
rizqicahyo 0:ac7353383a8e 344 gCase++;
rizqicahyo 1:c9f11055fb12 345 wait_ms(200);
rizqicahyo 0:ac7353383a8e 346 }
rizqicahyo 0:ac7353383a8e 347 break;
rizqicahyo 0:ac7353383a8e 348 }
rizqicahyo 0:ac7353383a8e 349 case 1 :
rizqicahyo 0:ac7353383a8e 350 {
rizqicahyo 0:ac7353383a8e 351 if (gMode == 1)
rizqicahyo 0:ac7353383a8e 352 {
rizqicahyo 0:ac7353383a8e 353 if ((ps2.read(PS_PAD::PAD_X)==1))
rizqicahyo 0:ac7353383a8e 354 {
rizqicahyo 0:ac7353383a8e 355 PIDrunning();
rizqicahyo 0:ac7353383a8e 356 pc.printf("PID \t %f \t ",PID.compute());
rizqicahyo 0:ac7353383a8e 357
rizqicahyo 0:ac7353383a8e 358 for(i=0;i<13;i++)
rizqicahyo 0:ac7353383a8e 359 {
rizqicahyo 0:ac7353383a8e 360 pc.printf("%i \t",sensor[i]);
rizqicahyo 0:ac7353383a8e 361 }
rizqicahyo 0:ac7353383a8e 362 pc.printf("\n");
rizqicahyo 0:ac7353383a8e 363 }
rizqicahyo 0:ac7353383a8e 364 else if((ps2.read(PS_PAD::PAD_X)==0))
rizqicahyo 0:ac7353383a8e 365 {
rizqicahyo 0:ac7353383a8e 366 motor1.brake(1);
rizqicahyo 0:ac7353383a8e 367 motor2.brake(1);
rizqicahyo 0:ac7353383a8e 368 pc.printf("PID stop \n");
rizqicahyo 0:ac7353383a8e 369 }
rizqicahyo 0:ac7353383a8e 370 }
rizqicahyo 0:ac7353383a8e 371 else if (gMode == 0)
rizqicahyo 0:ac7353383a8e 372 {
rizqicahyo 0:ac7353383a8e 373 if ((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0) && (ps2.read(PS_PAD::PAD_R1)==0) && (ps2.read(PS_PAD::PAD_L1)==0)) //maju
rizqicahyo 0:ac7353383a8e 374 {
rizqicahyo 0:ac7353383a8e 375 if (speed < 0.1)
rizqicahyo 0:ac7353383a8e 376 speed = 0.1;
rizqicahyo 0:ac7353383a8e 377 else
rizqicahyo 0:ac7353383a8e 378 speed += 0.0001;
rizqicahyo 0:ac7353383a8e 379
rizqicahyo 0:ac7353383a8e 380 if (speed >= gMax_speed)
rizqicahyo 0:ac7353383a8e 381 speed = gMax_speed;
rizqicahyo 0:ac7353383a8e 382
rizqicahyo 0:ac7353383a8e 383 motor1.speed(speed*k);
rizqicahyo 0:ac7353383a8e 384 motor2.speed(speed*k);
rizqicahyo 0:ac7353383a8e 385 pc.printf("maju \n");
rizqicahyo 0:ac7353383a8e 386 }
rizqicahyo 0:ac7353383a8e 387 else if ((ps2.read(PS_PAD::PAD_TOP)==0) && (ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_R1)==0) && (ps2.read(PS_PAD::PAD_L1)==0)) //mundur
rizqicahyo 0:ac7353383a8e 388 {
rizqicahyo 0:ac7353383a8e 389 if (speed < 0.1)
rizqicahyo 0:ac7353383a8e 390 speed = 0.1;
rizqicahyo 0:ac7353383a8e 391 else
rizqicahyo 0:ac7353383a8e 392 speed += 0.0001;
rizqicahyo 0:ac7353383a8e 393
rizqicahyo 0:ac7353383a8e 394 if (speed >= gMax_speed)
rizqicahyo 0:ac7353383a8e 395 speed = gMax_speed;
rizqicahyo 0:ac7353383a8e 396
rizqicahyo 0:ac7353383a8e 397 motor1.speed(-speed*k);
rizqicahyo 0:ac7353383a8e 398 motor2.speed(-speed*k);
rizqicahyo 0:ac7353383a8e 399 pc.printf("mundur \n");
rizqicahyo 0:ac7353383a8e 400 }
rizqicahyo 0:ac7353383a8e 401 else if ((ps2.read(PS_PAD::PAD_TOP)==0) && (ps2.read(PS_PAD::PAD_BOTTOM)==0) && (ps2.read(PS_PAD::PAD_R1)==0) && (ps2.read(PS_PAD::PAD_L1)==1)) //pivot kiri
rizqicahyo 0:ac7353383a8e 402 {
rizqicahyo 0:ac7353383a8e 403 if (speed < 0.1)
rizqicahyo 0:ac7353383a8e 404 speed = 0.1;
rizqicahyo 0:ac7353383a8e 405 else
rizqicahyo 0:ac7353383a8e 406 speed += 0.0001;
rizqicahyo 0:ac7353383a8e 407
rizqicahyo 0:ac7353383a8e 408 if (speed >= gMax_speed)
rizqicahyo 0:ac7353383a8e 409 speed = gMax_speed;
rizqicahyo 0:ac7353383a8e 410
rizqicahyo 0:ac7353383a8e 411 motor1.speed(speed*k*0.7);
rizqicahyo 0:ac7353383a8e 412 motor2.speed(-speed*k*0.7);
rizqicahyo 0:ac7353383a8e 413 pc.printf("piv kiri \n");
rizqicahyo 0:ac7353383a8e 414 }
rizqicahyo 0:ac7353383a8e 415 else if ((ps2.read(PS_PAD::PAD_TOP)==0) && (ps2.read(PS_PAD::PAD_BOTTOM)==0) && (ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)) //pivot kanan
rizqicahyo 0:ac7353383a8e 416 {
rizqicahyo 0:ac7353383a8e 417 if (speed < 0.1)
rizqicahyo 0:ac7353383a8e 418 speed = 0.1;
rizqicahyo 0:ac7353383a8e 419 else
rizqicahyo 0:ac7353383a8e 420 speed += 0.0001;
rizqicahyo 0:ac7353383a8e 421
rizqicahyo 0:ac7353383a8e 422 if (speed >= gMax_speed)
rizqicahyo 0:ac7353383a8e 423 speed = gMax_speed;
rizqicahyo 0:ac7353383a8e 424
rizqicahyo 1:c9f11055fb12 425 motor1.speed(speed*k*0.7-gTuning);
rizqicahyo 1:c9f11055fb12 426 motor2.speed(-speed*k*0.7);
rizqicahyo 0:ac7353383a8e 427 pc.printf("piv kanan \n");
rizqicahyo 0:ac7353383a8e 428 }
rizqicahyo 0:ac7353383a8e 429 else if ((ps2.read(PS_PAD::PAD_TOP)==0) && (ps2.read(PS_PAD::PAD_BOTTOM)==0) && (ps2.read(PS_PAD::PAD_R1)==0) && (ps2.read(PS_PAD::PAD_L1)==0))
rizqicahyo 0:ac7353383a8e 430 {
rizqicahyo 0:ac7353383a8e 431 motor1.brake(0);
rizqicahyo 0:ac7353383a8e 432 motor2.brake(0);
rizqicahyo 0:ac7353383a8e 433 pc.printf("stop \n");
rizqicahyo 0:ac7353383a8e 434 }
rizqicahyo 0:ac7353383a8e 435
rizqicahyo 0:ac7353383a8e 436 }
rizqicahyo 0:ac7353383a8e 437 else
rizqicahyo 0:ac7353383a8e 438 {
rizqicahyo 0:ac7353383a8e 439 motor1.brake(1);
rizqicahyo 0:ac7353383a8e 440 motor2.brake(1);
rizqicahyo 0:ac7353383a8e 441 pc.printf("no PID no MANUAL \n");
rizqicahyo 0:ac7353383a8e 442 }
rizqicahyo 0:ac7353383a8e 443
rizqicahyo 0:ac7353383a8e 444 if((ps2.read(PS_PAD::PAD_SELECT)==1) && (ps2.read(PS_PAD::PAD_START)==0))
rizqicahyo 0:ac7353383a8e 445 {
rizqicahyo 0:ac7353383a8e 446 if (gMode==1)
rizqicahyo 0:ac7353383a8e 447 {
rizqicahyo 0:ac7353383a8e 448 gMode=0;
rizqicahyo 1:c9f11055fb12 449 wait_ms(200);
rizqicahyo 0:ac7353383a8e 450 }
rizqicahyo 0:ac7353383a8e 451 else
rizqicahyo 0:ac7353383a8e 452 {
rizqicahyo 0:ac7353383a8e 453 gMode=1;
rizqicahyo 1:c9f11055fb12 454 wait_ms(200);
rizqicahyo 0:ac7353383a8e 455 }
rizqicahyo 0:ac7353383a8e 456 }
rizqicahyo 0:ac7353383a8e 457
rizqicahyo 0:ac7353383a8e 458 if((ps2.read(PS_PAD::PAD_START)==1) && (ps2.read(PS_PAD::PAD_SELECT)==0))
rizqicahyo 0:ac7353383a8e 459 {
rizqicahyo 0:ac7353383a8e 460 gCase--;
rizqicahyo 1:c9f11055fb12 461 wait_ms(200);
rizqicahyo 0:ac7353383a8e 462 }
rizqicahyo 0:ac7353383a8e 463
rizqicahyo 0:ac7353383a8e 464 break;
rizqicahyo 0:ac7353383a8e 465 }
rizqicahyo 0:ac7353383a8e 466 }
rizqicahyo 0:ac7353383a8e 467 }
rizqicahyo 0:ac7353383a8e 468
rizqicahyo 0:ac7353383a8e 469 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 470 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 471 /** PROGRAM UTAMA **/
rizqicahyo 0:ac7353383a8e 472 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 473 /*********************************************************************************************/
rizqicahyo 0:ac7353383a8e 474 int main(void){
rizqicahyo 0:ac7353383a8e 475 //inisialisasi joystick
rizqicahyo 0:ac7353383a8e 476 ps2.init();
rizqicahyo 0:ac7353383a8e 477
rizqicahyo 0:ac7353383a8e 478 //inisialisasi PID
rizqicahyo 0:ac7353383a8e 479 PID.setInputLimits(-15,15);
rizqicahyo 0:ac7353383a8e 480 PID.setOutputLimits(-1.0,1.0);
rizqicahyo 0:ac7353383a8e 481 PID.setMode(1.0);
rizqicahyo 0:ac7353383a8e 482 PID.setBias(0.0);
rizqicahyo 0:ac7353383a8e 483 PID.reset();
rizqicahyo 0:ac7353383a8e 484
rizqicahyo 0:ac7353383a8e 485 // serial PC
rizqicahyo 0:ac7353383a8e 486 pc.baud(115200);
rizqicahyo 0:ac7353383a8e 487
rizqicahyo 0:ac7353383a8e 488 //multitasking untuk menampilkan layar
rizqicahyo 1:c9f11055fb12 489 startMillis();
rizqicahyo 1:c9f11055fb12 490 uint32_t lastTime = 0;
rizqicahyo 1:c9f11055fb12 491 int lcd_refresh_rate = 10;
rizqicahyo 0:ac7353383a8e 492
rizqicahyo 0:ac7353383a8e 493 while(1)
rizqicahyo 0:ac7353383a8e 494 {
rizqicahyo 1:c9f11055fb12 495 if(millis() - lastTime >= lcd_refresh_rate)
rizqicahyo 1:c9f11055fb12 496 {
rizqicahyo 1:c9f11055fb12 497 showLCD;
rizqicahyo 1:c9f11055fb12 498 }
rizqicahyo 0:ac7353383a8e 499 init_sensor();
rizqicahyo 0:ac7353383a8e 500 ps2.poll();
rizqicahyo 0:ac7353383a8e 501 running();
rizqicahyo 0:ac7353383a8e 502 }
rizqicahyo 0:ac7353383a8e 503 }