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@0:ac7353383a8e, 2016-02-09 (annotated)

Committer:
rizqicahyo
Date:
Tue Feb 09 14:15:11 2016 +0000
Revision:
0:ac7353383a8e
Child:
1:c9f11055fb12
fisrt commit

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