KRAI 2017 / Mbed 2 deprecated BASE_NASIONAL_V2_12Juni

Belum semua kombinasi. Start = mode rotasi otomatis. Mode otomatis = L1 dan R1 untuk iterasi. Select = keluar mode rotasi otomatis.

Dependencies:   Motor PID mbed millis

Fork of BASE_NASIONAL_V2 by KRAI 2017

main.cpp@1:6503823263a4, 2017-06-12 (annotated)

Committer:
gustavaditya
Date:
Mon Jun 12 09:24:40 2017 +0000
Revision:
1:6503823263a4
Parent:
0:312d1d0781ac 
Base Siap

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gustavaditya 0:312d1d0781ac 1 #include "mbed.h"
gustavaditya 0:312d1d0781ac 2 #include "JoystickPS3.h"
gustavaditya 0:312d1d0781ac 3 #include "Motor.h"
gustavaditya 0:312d1d0781ac 4 #include "encoderKRAI.h"
gustavaditya 0:312d1d0781ac 5 #include "millis.h"
gustavaditya 0:312d1d0781ac 6
gustavaditya 0:312d1d0781ac 7 #define PI 3.14159265
gustavaditya 0:312d1d0781ac 8 #define D_ENCODER 10 // Diameter Roda Encoder
gustavaditya 0:312d1d0781ac 9 #define D_ROBOT1 54 // Diameter Roda Robot dari kiri ke kanan
gustavaditya 0:312d1d0781ac 10 #define D_ROBOT2 79 // Diameter Roda Robot dari depan ke belakang
gustavaditya 0:312d1d0781ac 11
gustavaditya 0:312d1d0781ac 12 bool auto_rotate=false, flag_start=true, flag_buttonL1, flag_buttonR1, flag_select;
gustavaditya 0:312d1d0781ac 13 bool reset_encoder = true;
gustavaditya 0:312d1d0781ac 14
gustavaditya 0:312d1d0781ac 15 int mode=0;
gustavaditya 0:312d1d0781ac 16
gustavaditya 0:312d1d0781ac 17 float V_x = 0.4, V_y = 0.3, V_pivot = 0.2;
gustavaditya 0:312d1d0781ac 18 float rasio= (D_ROBOT2/D_ROBOT1);
gustavaditya 0:312d1d0781ac 19
gustavaditya 0:312d1d0781ac 20 float K_enc = PI*D_ENCODER;
gustavaditya 0:312d1d0781ac 21 float K_robot1 = PI*D_ROBOT1;
gustavaditya 0:312d1d0781ac 22 float K_robot2 = PI*D_ROBOT2;
gustavaditya 0:312d1d0781ac 23
gustavaditya 0:312d1d0781ac 24 float PIDSpeedX, PIDSpeedY, PIDSpeedT;
gustavaditya 0:312d1d0781ac 25 float speedDpn, speedBlk, speedR, speedL;
gustavaditya 0:312d1d0781ac 26 const float maxPIDSpeed = 0.3, minPIDSpeed = -0.3, Ts = 50;
gustavaditya 0:312d1d0781ac 27 const float maxPIDSpeedT = 0.4, minPIDSpeedT = -0.4;
gustavaditya 0:312d1d0781ac 28 double current_error, previous_error1 = 0, previous_error2 = 0;
gustavaditya 0:312d1d0781ac 29 unsigned long int previousMillis=0;
gustavaditya 0:312d1d0781ac 30 float setpointX=0.0, setpointY=0.0, setpointT=0.0;
gustavaditya 0:312d1d0781ac 31
gustavaditya 0:312d1d0781ac 32 /* Untuk PID */
gustavaditya 0:312d1d0781ac 33 float errorX, previous_errorX=0, derivativeX, integralX=0;
gustavaditya 0:312d1d0781ac 34 float errorY, previous_errorY=0, derivativeY, integralY=0;
gustavaditya 0:312d1d0781ac 35 float errorT, previous_errorT=0, derivativeT, integralT=0;
gustavaditya 0:312d1d0781ac 36 float KpX=0.11, KiX=0, KdX=0;
gustavaditya 0:312d1d0781ac 37 float KpY=0.11, KiY=0, KdY=0;
gustavaditya 0:312d1d0781ac 38 float KpT=0.08, KiT=0, KdT=3.33;
gustavaditya 0:312d1d0781ac 39
gustavaditya 0:312d1d0781ac 40 /* Untuk joystick */
gustavaditya 0:312d1d0781ac 41 int case_joy;
gustavaditya 0:312d1d0781ac 42
gustavaditya 0:312d1d0781ac 43 /* Inisialisasi Pin TX-RX Joystik dan PC */
gustavaditya 0:312d1d0781ac 44 joysticknucleo joystick(PA_0,PA_1);
gustavaditya 0:312d1d0781ac 45 Serial pc(USBTX,USBRX);
gustavaditya 0:312d1d0781ac 46
gustavaditya 0:312d1d0781ac 47 /* Deklarasi Encoder Base */
gustavaditya 0:312d1d0781ac 48 encoderKRAI encKanan( PA_14, PA_15, 28, encoderKRAI::X4_ENCODING);
gustavaditya 0:312d1d0781ac 49 encoderKRAI encBlk( PB_4, PB_5, 28, encoderKRAI::X4_ENCODING);
gustavaditya 0:312d1d0781ac 50 encoderKRAI encDpn( PC_11, PC_10, 28, encoderKRAI::X4_ENCODING);
gustavaditya 0:312d1d0781ac 51 encoderKRAI encKiri( PC_12, PD_2, 28, encoderKRAI::X4_ENCODING);
gustavaditya 0:312d1d0781ac 52
gustavaditya 0:312d1d0781ac 53 /* Deklarasi Motor Base */
gustavaditya 0:312d1d0781ac 54 Motor motorDpn(PB_7, PC_3, PC_0);
gustavaditya 0:312d1d0781ac 55 Motor motorBlk(PB_2, PB_15, PB_1);
gustavaditya 0:312d1d0781ac 56 Motor motorL (PB_9, PA_12, PA_6);
gustavaditya 0:312d1d0781ac 57 Motor motorR (PB_8, PC_6, PC_5);
gustavaditya 0:312d1d0781ac 58
gustavaditya 0:312d1d0781ac 59 void setCenter()
gustavaditya 0:312d1d0781ac 60 {
gustavaditya 0:312d1d0781ac 61 /* Fungsi untuk menentukan center dari robot */
gustavaditya 0:312d1d0781ac 62 encDpn.reset();
gustavaditya 0:312d1d0781ac 63 encBlk.reset();
gustavaditya 0:312d1d0781ac 64 encKiri.reset();
gustavaditya 0:312d1d0781ac 65 encKanan.reset();
gustavaditya 0:312d1d0781ac 66 }
gustavaditya 0:312d1d0781ac 67
gustavaditya 0:312d1d0781ac 68 int case_joystick()
gustavaditya 0:312d1d0781ac 69 {
gustavaditya 0:312d1d0781ac 70 int caseJoystick;
gustavaditya 0:312d1d0781ac 71 if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
gustavaditya 0:312d1d0781ac 72 // Pivot Kanan
gustavaditya 0:312d1d0781ac 73 caseJoystick = 1;
gustavaditya 0:312d1d0781ac 74 }
gustavaditya 0:312d1d0781ac 75 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
gustavaditya 0:312d1d0781ac 76 // Pivot Kiri
gustavaditya 0:312d1d0781ac 77 caseJoystick = 2;
gustavaditya 0:312d1d0781ac 78 }
gustavaditya 0:312d1d0781ac 79 else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
gustavaditya 0:312d1d0781ac 80 // Kanan + Rotasi kanan
gustavaditya 0:312d1d0781ac 81 caseJoystick = 3;
gustavaditya 0:312d1d0781ac 82 }
gustavaditya 0:312d1d0781ac 83 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
gustavaditya 0:312d1d0781ac 84 // Kanan + Rotasi kiri
gustavaditya 0:312d1d0781ac 85 caseJoystick = 4;
gustavaditya 0:312d1d0781ac 86 }
gustavaditya 0:312d1d0781ac 87 else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
gustavaditya 0:312d1d0781ac 88 // Kiri + Rotasi kanan
gustavaditya 0:312d1d0781ac 89 caseJoystick = 5;
gustavaditya 0:312d1d0781ac 90 }
gustavaditya 0:312d1d0781ac 91 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
gustavaditya 0:312d1d0781ac 92 // Kiri + Rotasi kiri
gustavaditya 0:312d1d0781ac 93 caseJoystick = 6;
gustavaditya 0:312d1d0781ac 94 }
gustavaditya 0:312d1d0781ac 95 else if ((joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
gustavaditya 0:312d1d0781ac 96 // Maju + Rotasi kanan
gustavaditya 0:312d1d0781ac 97 caseJoystick = 7;
gustavaditya 0:312d1d0781ac 98 }
gustavaditya 0:312d1d0781ac 99 else if ((!joystick.R1)&&(joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
gustavaditya 0:312d1d0781ac 100 // Maju + Rotasi kiri
gustavaditya 0:312d1d0781ac 101 caseJoystick = 8;
gustavaditya 0:312d1d0781ac 102 }
gustavaditya 0:312d1d0781ac 103 else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
gustavaditya 0:312d1d0781ac 104 // Mundur + Rotasi kanan
gustavaditya 0:312d1d0781ac 105 caseJoystick = 9;
gustavaditya 0:312d1d0781ac 106 }
gustavaditya 0:312d1d0781ac 107 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
gustavaditya 0:312d1d0781ac 108 // Mundur + Rotasi kiri
gustavaditya 0:312d1d0781ac 109 caseJoystick = 10;
gustavaditya 0:312d1d0781ac 110 }
gustavaditya 0:312d1d0781ac 111 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
gustavaditya 0:312d1d0781ac 112 // Kanan
gustavaditya 0:312d1d0781ac 113 caseJoystick = 11;
gustavaditya 0:312d1d0781ac 114 }
gustavaditya 0:312d1d0781ac 115 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
gustavaditya 0:312d1d0781ac 116 // Kiri
gustavaditya 0:312d1d0781ac 117 caseJoystick = 12;
gustavaditya 0:312d1d0781ac 118 }
gustavaditya 0:312d1d0781ac 119 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
gustavaditya 0:312d1d0781ac 120 // Maju
gustavaditya 0:312d1d0781ac 121 caseJoystick = 13;
gustavaditya 0:312d1d0781ac 122 }
gustavaditya 0:312d1d0781ac 123 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
gustavaditya 0:312d1d0781ac 124 // Mundur
gustavaditya 0:312d1d0781ac 125 caseJoystick = 14;
gustavaditya 0:312d1d0781ac 126 }
gustavaditya 0:312d1d0781ac 127
gustavaditya 0:312d1d0781ac 128 return(caseJoystick);
gustavaditya 0:312d1d0781ac 129 }
gustavaditya 0:312d1d0781ac 130
gustavaditya 0:312d1d0781ac 131 void aktuator()
gustavaditya 0:312d1d0781ac 132 {
gustavaditya 0:312d1d0781ac 133 switch (case_joy) {
gustavaditya 0:312d1d0781ac 134 case (1):
gustavaditya 0:312d1d0781ac 135 // Pivot Kanan
gustavaditya 0:312d1d0781ac 136 motorDpn.speed(-V_pivot/rasio);
gustavaditya 0:312d1d0781ac 137 motorBlk.speed(-V_pivot/rasio);
gustavaditya 0:312d1d0781ac 138 motorR.speed(-V_pivot);
gustavaditya 0:312d1d0781ac 139 motorL.speed(-V_pivot);
gustavaditya 0:312d1d0781ac 140 break;
gustavaditya 0:312d1d0781ac 141 case (2):
gustavaditya 0:312d1d0781ac 142 // Pivot Kiri
gustavaditya 0:312d1d0781ac 143 motorDpn.speed(V_pivot/rasio);
gustavaditya 0:312d1d0781ac 144 motorBlk.speed(V_pivot/rasio);
gustavaditya 0:312d1d0781ac 145 motorR.speed(V_pivot);
gustavaditya 0:312d1d0781ac 146 motorL.speed(V_pivot);
gustavaditya 0:312d1d0781ac 147 break;
gustavaditya 0:312d1d0781ac 148 case (11):
gustavaditya 0:312d1d0781ac 149 // Kanan
gustavaditya 0:312d1d0781ac 150 /*speedDpn = -(V_x + PIDSpeedT);
gustavaditya 0:312d1d0781ac 151 speedBlk = (V_x + PIDSpeedT);
gustavaditya 0:312d1d0781ac 152 speedR = rasio*PIDSpeedT + PIDSpeedY;
gustavaditya 0:312d1d0781ac 153 speedL = rasio*PIDSpeedT - PIDSpeedY;*/
gustavaditya 0:312d1d0781ac 154 speedDpn = -(V_x);
gustavaditya 0:312d1d0781ac 155 speedBlk = (V_x);
gustavaditya 0:312d1d0781ac 156 //speedR = PIDSpeedY;
gustavaditya 0:312d1d0781ac 157 //speedL = - PIDSpeedY;
gustavaditya 0:312d1d0781ac 158 motorDpn.speed(speedDpn);
gustavaditya 0:312d1d0781ac 159 motorBlk.speed(speedBlk);
gustavaditya 0:312d1d0781ac 160 motorR.brake(1);
gustavaditya 0:312d1d0781ac 161 motorL.brake(1);
gustavaditya 0:312d1d0781ac 162 break;
gustavaditya 0:312d1d0781ac 163 case (12):
gustavaditya 0:312d1d0781ac 164 // Kiri
gustavaditya 0:312d1d0781ac 165 /*speedDpn = (V_x + PIDSpeedT);
gustavaditya 0:312d1d0781ac 166 speedBlk = -(V_x + PIDSpeedT);
gustavaditya 0:312d1d0781ac 167 speedR = rasio*PIDSpeedT + PIDSpeedY;
gustavaditya 0:312d1d0781ac 168 speedL = rasio*PIDSpeedT - PIDSpeedY;*/
gustavaditya 0:312d1d0781ac 169 speedDpn = (V_x);
gustavaditya 0:312d1d0781ac 170 speedBlk = -(V_x);
gustavaditya 0:312d1d0781ac 171 //speedR = PIDSpeedY;
gustavaditya 0:312d1d0781ac 172 //speedL = -PIDSpeedY;
gustavaditya 0:312d1d0781ac 173 motorDpn.speed(speedDpn);
gustavaditya 0:312d1d0781ac 174 motorBlk.speed(speedBlk);
gustavaditya 0:312d1d0781ac 175 motorR.brake(1);
gustavaditya 0:312d1d0781ac 176 motorL.brake(1);
gustavaditya 0:312d1d0781ac 177 break;
gustavaditya 0:312d1d0781ac 178 case (13):
gustavaditya 0:312d1d0781ac 179 // Maju
gustavaditya 0:312d1d0781ac 180 motorDpn.brake(1);
gustavaditya 0:312d1d0781ac 181 motorBlk.brake(1);
gustavaditya 0:312d1d0781ac 182 motorR.speed(0.3);
gustavaditya 0:312d1d0781ac 183 motorL.speed(-0.3);
gustavaditya 0:312d1d0781ac 184 break;
gustavaditya 0:312d1d0781ac 185 case (14):
gustavaditya 0:312d1d0781ac 186 // Mundur
gustavaditya 0:312d1d0781ac 187 motorDpn.brake(1);
gustavaditya 0:312d1d0781ac 188 motorBlk.brake(1);
gustavaditya 0:312d1d0781ac 189 motorR.speed(-0.3);
gustavaditya 0:312d1d0781ac 190 motorL.speed(0.3);
gustavaditya 0:312d1d0781ac 191 break;
gustavaditya 0:312d1d0781ac 192 default :
gustavaditya 0:312d1d0781ac 193 motorDpn.brake(1);
gustavaditya 0:312d1d0781ac 194 motorBlk.brake(1);
gustavaditya 0:312d1d0781ac 195 motorR.brake(1);
gustavaditya 0:312d1d0781ac 196 motorL.brake(1);
gustavaditya 0:312d1d0781ac 197 break;
gustavaditya 0:312d1d0781ac 198 } // End Switch
gustavaditya 0:312d1d0781ac 199 }
gustavaditya 0:312d1d0781ac 200
gustavaditya 0:312d1d0781ac 201 float getX()
gustavaditya 0:312d1d0781ac 202 {
gustavaditya 0:312d1d0781ac 203 float jarakEncDpn, jarakEncBlk;
gustavaditya 0:312d1d0781ac 204 jarakEncDpn = (encDpn.getPulses())/(float)(540.0)*K_enc;
gustavaditya 0:312d1d0781ac 205 jarakEncBlk = (encBlk.getPulses())/(float)(540.0)*K_enc;
gustavaditya 0:312d1d0781ac 206 return (jarakEncBlk-jarakEncDpn)/2;
gustavaditya 0:312d1d0781ac 207 }
gustavaditya 0:312d1d0781ac 208
gustavaditya 0:312d1d0781ac 209 float getY()
gustavaditya 0:312d1d0781ac 210 {
gustavaditya 0:312d1d0781ac 211 float jarakEncKir, jarakEncKan;
gustavaditya 0:312d1d0781ac 212 jarakEncKir = (encKiri.getPulses())/(float)(540.0)*K_enc;
gustavaditya 0:312d1d0781ac 213 jarakEncKan = (encKanan.getPulses())/(float)(540.0)*K_enc;
gustavaditya 0:312d1d0781ac 214 return (jarakEncKan-jarakEncKir)/2;
gustavaditya 0:312d1d0781ac 215 }
gustavaditya 0:312d1d0781ac 216
gustavaditya 0:312d1d0781ac 217 float getTetha()
gustavaditya 0:312d1d0781ac 218 {
gustavaditya 0:312d1d0781ac 219 float busurDpn, busurBlk, busurKir, busurKan, sudut;
gustavaditya 0:312d1d0781ac 220 busurKir = ((encKiri.getPulses())/(float)(540.0)*K_enc)/K_robot2*360.0;
gustavaditya 0:312d1d0781ac 221 busurKan = ((encKanan.getPulses())/(float)(540.0)*K_enc)/K_robot2*360.0;
gustavaditya 0:312d1d0781ac 222 busurDpn = ((encDpn.getPulses())/(float)(540.0)*K_enc)/K_robot1*360.0;
gustavaditya 0:312d1d0781ac 223 busurBlk = ((encBlk.getPulses())/(float)(540.0)*K_enc)/K_robot1*360.0;
gustavaditya 0:312d1d0781ac 224 sudut = (busurDpn+busurBlk+busurKir+busurKan)/4;
gustavaditya 0:312d1d0781ac 225 if (sudut>=360.0 || sudut<=-360.0)
gustavaditya 0:312d1d0781ac 226 {
gustavaditya 0:312d1d0781ac 227 sudut = 0.0;
gustavaditya 0:312d1d0781ac 228 }
gustavaditya 0:312d1d0781ac 229 return sudut;
gustavaditya 0:312d1d0781ac 230 }
gustavaditya 0:312d1d0781ac 231
gustavaditya 0:312d1d0781ac 232 void PIDcompute()
gustavaditya 0:312d1d0781ac 233 {
gustavaditya 0:312d1d0781ac 234 errorX = setpointX - getX();
gustavaditya 0:312d1d0781ac 235 errorY = setpointY - getY();
gustavaditya 0:312d1d0781ac 236 errorT = setpointT - getTetha();
gustavaditya 0:312d1d0781ac 237
gustavaditya 0:312d1d0781ac 238 integralX = integralX + errorX*Ts;
gustavaditya 0:312d1d0781ac 239 integralY = integralY + errorY*Ts;
gustavaditya 0:312d1d0781ac 240 integralT = integralT + errorT*Ts;
gustavaditya 0:312d1d0781ac 241
gustavaditya 0:312d1d0781ac 242 derivativeX = (errorX - previous_errorX)/Ts;
gustavaditya 0:312d1d0781ac 243 derivativeY = (errorY - previous_errorY)/Ts;
gustavaditya 0:312d1d0781ac 244 derivativeT = (errorT - previous_errorT)/Ts;
gustavaditya 0:312d1d0781ac 245
gustavaditya 0:312d1d0781ac 246 PIDSpeedX = KpX*errorY + KiX*integralY + KdX*derivativeX;
gustavaditya 0:312d1d0781ac 247 PIDSpeedY = KpY*errorY + KiY*integralY + KdY*derivativeY;
gustavaditya 0:312d1d0781ac 248 PIDSpeedT = KpT*errorT + KiT*integralT + KdT*derivativeT;
gustavaditya 0:312d1d0781ac 249
gustavaditya 0:312d1d0781ac 250 previous_errorX = errorX;
gustavaditya 0:312d1d0781ac 251 previous_errorY = errorY;
gustavaditya 0:312d1d0781ac 252 previous_errorT = errorT;
gustavaditya 0:312d1d0781ac 253
gustavaditya 0:312d1d0781ac 254 if(PIDSpeedX > maxPIDSpeed){
gustavaditya 0:312d1d0781ac 255 PIDSpeedX = maxPIDSpeed;
gustavaditya 0:312d1d0781ac 256 }
gustavaditya 0:312d1d0781ac 257 else if (PIDSpeedX < minPIDSpeed){
gustavaditya 0:312d1d0781ac 258 PIDSpeedX = minPIDSpeed;
gustavaditya 0:312d1d0781ac 259 }
gustavaditya 0:312d1d0781ac 260
gustavaditya 0:312d1d0781ac 261 if(PIDSpeedY > maxPIDSpeed){
gustavaditya 0:312d1d0781ac 262 PIDSpeedY = maxPIDSpeed;
gustavaditya 0:312d1d0781ac 263 }
gustavaditya 0:312d1d0781ac 264 else if (PIDSpeedY < minPIDSpeed){
gustavaditya 0:312d1d0781ac 265 PIDSpeedY = minPIDSpeed;
gustavaditya 0:312d1d0781ac 266 }
gustavaditya 0:312d1d0781ac 267
gustavaditya 0:312d1d0781ac 268 if(PIDSpeedT > maxPIDSpeedT){
gustavaditya 0:312d1d0781ac 269 PIDSpeedT = maxPIDSpeedT;
gustavaditya 0:312d1d0781ac 270 }
gustavaditya 0:312d1d0781ac 271 else if (PIDSpeedT < minPIDSpeedT){
gustavaditya 0:312d1d0781ac 272 PIDSpeedT = minPIDSpeedT;
gustavaditya 0:312d1d0781ac 273 }
gustavaditya 0:312d1d0781ac 274 }
gustavaditya 0:312d1d0781ac 275
gustavaditya 0:312d1d0781ac 276 void aturSetpoint()
gustavaditya 0:312d1d0781ac 277 {
gustavaditya 0:312d1d0781ac 278 switch (mode)
gustavaditya 0:312d1d0781ac 279 {
gustavaditya 0:312d1d0781ac 280 case (-2):
gustavaditya 0:312d1d0781ac 281 setpointT = -45.0;
gustavaditya 0:312d1d0781ac 282 break;
gustavaditya 0:312d1d0781ac 283 case (-1):
gustavaditya 0:312d1d0781ac 284 setpointT = -20.0;
gustavaditya 0:312d1d0781ac 285 break;
gustavaditya 0:312d1d0781ac 286 case (0):
gustavaditya 0:312d1d0781ac 287 setpointT = 0.0;
gustavaditya 0:312d1d0781ac 288 break;
gustavaditya 0:312d1d0781ac 289 case (1):
gustavaditya 0:312d1d0781ac 290 setpointT = 20.0;
gustavaditya 0:312d1d0781ac 291 break;
gustavaditya 0:312d1d0781ac 292 case (2):
gustavaditya 0:312d1d0781ac 293 setpointT = 45.0;
gustavaditya 0:312d1d0781ac 294 break;
gustavaditya 0:312d1d0781ac 295 }
gustavaditya 0:312d1d0781ac 296 }
gustavaditya 0:312d1d0781ac 297 void cekMode()
gustavaditya 0:312d1d0781ac 298 {
gustavaditya 0:312d1d0781ac 299 if (mode<-2){mode = -2;}
gustavaditya 0:312d1d0781ac 300 else if (mode>2){mode = 2;}
gustavaditya 0:312d1d0781ac 301 }
gustavaditya 0:312d1d0781ac 302
gustavaditya 0:312d1d0781ac 303 void case_autoRotate()
gustavaditya 0:312d1d0781ac 304 {
gustavaditya 0:312d1d0781ac 305 if ((joystick.R1_click)&&(!joystick.L1_click)&&flag_buttonR1)
gustavaditya 0:312d1d0781ac 306 {
gustavaditya 0:312d1d0781ac 307 flag_buttonR1 = false;
gustavaditya 0:312d1d0781ac 308 mode--;
gustavaditya 0:312d1d0781ac 309 cekMode();
gustavaditya 0:312d1d0781ac 310 pc.printf("masuk R1, mode = %d\n",mode);
gustavaditya 0:312d1d0781ac 311 }
gustavaditya 0:312d1d0781ac 312 else { flag_buttonR1 = true; }
gustavaditya 0:312d1d0781ac 313
gustavaditya 0:312d1d0781ac 314 if ((!joystick.R1_click)&&(joystick.L1_click)&&flag_buttonL1)
gustavaditya 0:312d1d0781ac 315 {
gustavaditya 0:312d1d0781ac 316 flag_buttonL1 = false;
gustavaditya 0:312d1d0781ac 317 mode++;
gustavaditya 0:312d1d0781ac 318 cekMode();
gustavaditya 0:312d1d0781ac 319 pc.printf("masuk L1, mode = %d\n",mode);
gustavaditya 0:312d1d0781ac 320 }
gustavaditya 0:312d1d0781ac 321 else { flag_buttonL1 = true; }
gustavaditya 0:312d1d0781ac 322
gustavaditya 0:312d1d0781ac 323 if ((joystick.START_click)&&(!joystick.SELECT_click)&&flag_start)
gustavaditya 0:312d1d0781ac 324 {
gustavaditya 0:312d1d0781ac 325 // Masuk auto rotate
gustavaditya 0:312d1d0781ac 326 flag_start = false;
gustavaditya 0:312d1d0781ac 327 auto_rotate = true;
gustavaditya 0:312d1d0781ac 328 flag_buttonR1 = true;
gustavaditya 0:312d1d0781ac 329 flag_buttonL1 = true;
gustavaditya 0:312d1d0781ac 330 flag_select = true;
gustavaditya 0:312d1d0781ac 331 pc.printf("MASUK AUTO ROTATE");
gustavaditya 0:312d1d0781ac 332 }
gustavaditya 0:312d1d0781ac 333 else { flag_start = true; }
gustavaditya 0:312d1d0781ac 334
gustavaditya 0:312d1d0781ac 335 if ((!joystick.START_click)&&(joystick.SELECT_click)&&flag_select)
gustavaditya 0:312d1d0781ac 336 {
gustavaditya 0:312d1d0781ac 337 // Keluar auto rotate
gustavaditya 0:312d1d0781ac 338 flag_select = false;
gustavaditya 0:312d1d0781ac 339 auto_rotate = false;
gustavaditya 0:312d1d0781ac 340 reset_encoder = true;
gustavaditya 0:312d1d0781ac 341 setpointT = 0.0;
gustavaditya 0:312d1d0781ac 342 mode = 0;
gustavaditya 0:312d1d0781ac 343 pc.printf("KELUAR AUTO ROTATE");
gustavaditya 0:312d1d0781ac 344 }
gustavaditya 0:312d1d0781ac 345 else { flag_select = true; }
gustavaditya 0:312d1d0781ac 346 }
gustavaditya 0:312d1d0781ac 347
gustavaditya 0:312d1d0781ac 348 int main(void)
gustavaditya 0:312d1d0781ac 349 {
gustavaditya 0:312d1d0781ac 350 joystick.setup();
gustavaditya 0:312d1d0781ac 351 pc.baud(115200);
gustavaditya 0:312d1d0781ac 352 wait_ms(1000);
gustavaditya 0:312d1d0781ac 353 startMillis();
gustavaditya 0:312d1d0781ac 354 while(1)
gustavaditya 0:312d1d0781ac 355 {
gustavaditya 0:312d1d0781ac 356 //COBA ROTASI
gustavaditya 0:312d1d0781ac 357 joystick.idle();
gustavaditya 0:312d1d0781ac 358 if(joystick.readable())
gustavaditya 0:312d1d0781ac 359 {
gustavaditya 0:312d1d0781ac 360 // Panggil fungsi pembacaan joystik
gustavaditya 0:312d1d0781ac 361 joystick.baca_data();
gustavaditya 0:312d1d0781ac 362 // Panggil fungsi pengolahan data joystik
gustavaditya 0:312d1d0781ac 363 joystick.olah_data();
gustavaditya 0:312d1d0781ac 364 // Masuk ke case joystick
gustavaditya 0:312d1d0781ac 365 case_joy = case_joystick();
gustavaditya 0:312d1d0781ac 366 aktuator();
gustavaditya 0:312d1d0781ac 367
gustavaditya 0:312d1d0781ac 368 case_autoRotate();
gustavaditya 0:312d1d0781ac 369
gustavaditya 0:312d1d0781ac 370 while(auto_rotate)
gustavaditya 0:312d1d0781ac 371 {
gustavaditya 0:312d1d0781ac 372 joystick.idle();
gustavaditya 0:312d1d0781ac 373 if(joystick.readable())
gustavaditya 0:312d1d0781ac 374 {
gustavaditya 0:312d1d0781ac 375 joystick.baca_data();
gustavaditya 0:312d1d0781ac 376 joystick.olah_data();
gustavaditya 0:312d1d0781ac 377
gustavaditya 0:312d1d0781ac 378 if (reset_encoder){ setCenter(); reset_encoder = false; }
gustavaditya 0:312d1d0781ac 379
gustavaditya 0:312d1d0781ac 380 case_autoRotate();
gustavaditya 0:312d1d0781ac 381 aturSetpoint();
gustavaditya 0:312d1d0781ac 382
gustavaditya 0:312d1d0781ac 383 while (millis()-previousMillis>=Ts)
gustavaditya 0:312d1d0781ac 384 { PIDcompute(); previousMillis = millis(); }
gustavaditya 0:312d1d0781ac 385
gustavaditya 0:312d1d0781ac 386 //speedDpn = PIDSpeedT/rasio - PIDSpeedX;
gustavaditya 0:312d1d0781ac 387 //speedBlk = PIDSpeedT/rasio + PIDSpeedX;
gustavaditya 0:312d1d0781ac 388 //speedR = PIDSpeedT + PIDSpeedY;
gustavaditya 0:312d1d0781ac 389 //speedL = PIDSpeedT - PIDSpeedY;
gustavaditya 0:312d1d0781ac 390
gustavaditya 0:312d1d0781ac 391 speedDpn = PIDSpeedT;
gustavaditya 0:312d1d0781ac 392 speedBlk = PIDSpeedT;
gustavaditya 0:312d1d0781ac 393 speedR = PIDSpeedT*rasio;
gustavaditya 0:312d1d0781ac 394 speedL = PIDSpeedT*rasio;
gustavaditya 0:312d1d0781ac 395
gustavaditya 0:312d1d0781ac 396 if ((errorT > 0.5) || (errorT<-0.5))
gustavaditya 0:312d1d0781ac 397 {
gustavaditya 0:312d1d0781ac 398 motorDpn.speed(speedDpn);
gustavaditya 0:312d1d0781ac 399 motorBlk.speed(speedBlk);
gustavaditya 0:312d1d0781ac 400 motorR.speed(speedR);
gustavaditya 0:312d1d0781ac 401 motorL.speed(speedL);
gustavaditya 0:312d1d0781ac 402 }
gustavaditya 0:312d1d0781ac 403 else
gustavaditya 0:312d1d0781ac 404 {
gustavaditya 0:312d1d0781ac 405 motorDpn.brake(1);
gustavaditya 0:312d1d0781ac 406 motorBlk.brake(1);
gustavaditya 0:312d1d0781ac 407 motorR.brake(1);
gustavaditya 0:312d1d0781ac 408 motorL.brake(1);
gustavaditya 0:312d1d0781ac 409 }
gustavaditya 0:312d1d0781ac 410 pc.printf("SUDUT = %f\n",getTetha());
gustavaditya 0:312d1d0781ac 411 case_autoRotate();
gustavaditya 0:312d1d0781ac 412 }
gustavaditya 0:312d1d0781ac 413 else
gustavaditya 0:312d1d0781ac 414 {
gustavaditya 0:312d1d0781ac 415 joystick.idle();
gustavaditya 0:312d1d0781ac 416 }
gustavaditya 0:312d1d0781ac 417 }
gustavaditya 0:312d1d0781ac 418 }
gustavaditya 0:312d1d0781ac 419 else
gustavaditya 0:312d1d0781ac 420 {
gustavaditya 0:312d1d0781ac 421 joystick.idle();
gustavaditya 0:312d1d0781ac 422 }
gustavaditya 0:312d1d0781ac 423 }
gustavaditya 0:312d1d0781ac 424 }