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
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?
User | Revision | Line number | New 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 | } |