KRAI 2017
Pakai akselerasi
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of
MainProgram_BaseBaru_fix_omni_5April_23-04
by 
KRAI 2017
Diff: main.cpp
- Revision:
- 31:d5cbda07fd95
- Parent:
- 30:d69cc27ac644
- Child:
- 32:581d4a2373f0
diff -r d69cc27ac644 -r d5cbda07fd95 main.cpp
--- a/main.cpp Sun Feb 12 11:19:48 2017 +0000
+++ b/main.cpp Mon Feb 13 12:03:37 2017 +0000
@@ -1,6 +1,8 @@
/****************************************************************************/
/* PROGRAM UNTUK PID CLOSED LOOP */
/* */
+/* Last Update : 13 Februari 2017 */
+/* */
/* - Digunakan encoder autonics */
/* - Konfigurasi Motor dan Encoder sbb : */
/* ______________________ */
@@ -49,88 +51,172 @@
#define PI 3.14159265
#define D_ENCODER 10 // Diameter Roda Encoder
#define D_ROBOT 80 // Diameter Roda Robot
-#define VMAX 0.3 // Kiri Kanan
-#define PIVOT 0.4 // Pivka, Pivki
#define PERPINDAHAN 1 // Perpindahan ke kanan dan kiri
// Variable Atas
-double speed,speed2, maxSpeed = 0.8, minSpeed = 0;
+double speed, speed2, maxSpeed = 0.8, minSpeed = 0;
double kpA=0.6757, kdA=0.6757, kiA=0.00006757;
double p,i,d;
double p2,i2,d2;
double last_error = 0, current_error, sum_error = 0;
double last_error2 = 0, current_error2, sum_error2 = 0;
-float rpm, target_rpm = 2.0;
-float rpm2, target_rpm2 = 4.0;
+float rpm, target_rpm = 8.0;
+float rpm2, target_rpm2 = 8.0;
+float pwmPowerUp = 0.5;
+float pwmPowerDown = -0.5;
bool henti=false, hentis=false;
// Variable Bawah
float Vt;
-float k_enc = PI*D_ENCODER;
-float k_robot = PI*D_ROBOT;
-float speedT = 0.2;
-float vpid = 0;
-float pwmP = 0.5;
-float pwmT = -0.95;
+float keliling_enc = PI*D_ENCODER;
+float keliling_robot = PI*D_ROBOT;
+float speedT = 0.2;
+float vpid = 0;
+float PIVOT = 0.4; // PWM Pivot Kanan, Pivot Kiri
+float tuneDpn = 0.365; // Tunning PWM motor Depan
+float tuneBlk = 0.46; // Tunning PWM motor belakang
+
+/* Variabel Encoder Bawah */
+float errTetha, Tetha; // Variabel yang didapatkan encoder
+
+/* Deklarasi Variable Millis */
+unsigned long int previousMillis = 0;
+unsigned long int currentMillis;
+unsigned long int previousMillis2 = 0;
+unsigned long int currentMillis2;
+unsigned long int previousMillis3 = 0;
+/* Variabel Stick */
+//Logic untuk masuk aktuator
+int case_joy;
+bool isLauncher = false;
+bool isReload = false;
+int flagLauncher = 1;
+bool flag_Pneu = false;
+
+/*****************************************************/
+/* Definisi Prosedur, Fungsi dan Setting Pinout */
+/*****************************************************/
+
+/* Fungsi dan Procedur Encoder */
+void init_speed(); //
+void aktuator(); // Pergerakan aktuator berdasarkan case joystick
+int case_joystick(); // Mendapatkan case dari joystick
+//void speedKalibrasiMotor(); // Pertambahan target RPM motor atas melalui joystick
+void setCenter(); // Prosedur reset encoder, posisi saat itu diset jadi titik (0,0)
+float getTetha(); // Fungsi mendapatkan error Tetha
+
+/* Inisialisasi Pin TX-RX Joystik dan PC */
+joysticknucleo joystick(PA_0,PA_1);
+Serial pc(USBTX,USBRX);
/* Deklarasi Encoder Base */
-encoderKRAI encoderKiri(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
+encoderKRAI encoderBase(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), 2xresolusi, mode pembacaan
/* Deklarasi Encoder Launcher */
-encoderKRAI encoderAtas2( PC_10, PC_11, 28, encoderKRAI::X2_ENCODING);
-encoderKRAI encoderAtas( PD_2, PC_12, 28, encoderKRAI::X2_ENCODING);
+encoderKRAI encLauncherBlk( PC_10, PC_11, 28, encoderKRAI::X2_ENCODING);
+encoderKRAI encLauncherDpn( PD_2, PC_12, 28, encoderKRAI::X2_ENCODING);
/* Deklarasi Motor Base */
-Motor motor1(PB_9, PA_12, PC_5); // pwm, fwd, rev, Motor Depan
-Motor motor2(PB_6, PB_1, PB_12); // pwm, fwd, rev, Motor Belakang
+Motor motorDpn(PB_9, PA_12, PC_5); // pwm, fwd, rev
+Motor motorBlk(PB_6, PB_1, PB_12); // pwm, fwd, rev
/* Deklarasi Motor Launcher */
-Motor motorL1(PA_8,PC_2,PC_1); // pwm ,fwd, rev, Motor Launcher1
-Motor motorL3(PA_10, PC_3, PC_0); // pwm, fwd, rev, Motor Launcher2
-Motor motorP(PB_7, PA_14, PA_15); // pwm, fwd, rev, Motor Powerscrew
+Motor launcherDpn(PA_8,PC_2,PC_1); // pwm ,fwd, rev
+Motor launcherBlk(PA_10, PC_3, PC_0); // pwm, fwd, rev
+Motor powerScrew(PB_7, PA_14, PA_15); // pwm, fwd, rev
/* Deklarasi Penumatik Launcher */
DigitalOut pneumatik(PB_3, PullUp);
/*Dekalrasi Limit Switch */
-DigitalIn limitA(PC_9, PullUp);
-DigitalIn limitT(PB_10, PullUp);
-DigitalIn limitB(PC_8, PullUp);
-
-/**
- * posX dan posY berdasarkan arah robot
- * encoder Depan & Belakang sejajar sumbu Y
- * encoder Kanan & Kirim sejajar sumbu X
- **/
-
-/* Variabel Encoder */
-float errT, Tetha; // Variabel yang didapatkan encoder
+DigitalIn limitAtas(PC_9, PullUp);
+DigitalIn limitTengah(PB_10, PullUp);
+DigitalIn limitBawah(PC_8, PullUp);
-/* Fungsi dan Procedur Encoder */
-void setCenter(); // Fungsi reset agar robot di tengah
-float getTetha(); // Fungsi mendapatkan jarak Tetha
-
-/* Inisialisasi Pin TX-RX Joystik dan PC */
-joysticknucleo joystick(PA_0,PA_1);
-Serial pc(USBTX,USBRX);
-
-/* Deklarasi Variable Milis */
-unsigned long int previousMillis = 0;
-unsigned long int currentMillis;
-unsigned long int previousMillis2 = 0;
-unsigned long int currentMillis2;
-
-/* Variabel Stick */
-char case_ger;
-bool launcher = false;
-bool reload = false;
/****************************************************/
/* Deklarasi Fungsi dan Procedure */
/****************************************************/
-void init_speed();
-void speedKalibrasiMotor();
+int case_joystick()
+{
+//---------------------------------------------------//
+// Gerak Motor Base //
+// Case 1 : Pivot kanan //
+// Case 2 : Pivot Kiri //
+// Case 3 : Kanan //
+// Case 4 : Kiri //
+// Case 5 : Break //
+//---------------------------------------------------//
+
+ int caseJoystick;
+ if (!joystick.L1 && joystick.R1) {
+ // Pivot Kanan
+ caseJoystick = 1;
+ }
+ else if (!joystick.R1 && joystick.L1) {
+ // Pivot Kiri
+ caseJoystick = 2;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
+ // Kanan
+ caseJoystick = 3;
+ pc.printf("kanan");
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
+ // Kiri
+ caseJoystick = 4;
+ pc.printf("kiri");
+ }
+ else if (joystick.segitiga_click && flagLauncher){
+ // Motor Launcher
+ caseJoystick = 5;
+ }
+ else if (joystick.R2_click && (target_rpm2 < 14)){
+ // Target Pulse PID ++ Motor Depan
+ caseJoystick = 6;
+ }
+ else if (joystick.L2_click && (target_rpm2 > 1)){
+ // Target Pulse PID -- Motor Depan
+ caseJoystick = 7;
+ }
+ else if (joystick.R3_click && (target_rpm < 14)){
+ // Target Pulse PID ++ Motor Belakang
+ caseJoystick = 8;
+ }
+ else if (joystick.L3_click && (target_rpm > 1)){
+ // Target Pulse PID -- Motor Belakang
+ caseJoystick = 9;
+ }
+ else if (joystick.silang_click){
+ // Pnemuatik ON
+ caseJoystick = 10;
+ }
+ else if ((joystick.atas)&&(!joystick.bawah)) {
+ // Power Screw Up
+ caseJoystick = 11;
+ }
+ else if ((!joystick.atas)&&(joystick.bawah)) {
+ // Power Screw Down
+ caseJoystick = 12;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ // Break
+ caseJoystick = 13;
+ }
+
+ return(caseJoystick);
+}
+
+float getTetha(){
+// Fungsi untuk mendapatkan nilai tetha
+ float busur, tetha;
+ busur = ((encoderBase.getPulses())/(float)(2000.0)*keliling_enc);
+ tetha = busur/keliling_robot*360;
+
+ return -(tetha);
+}
+
float pidBase(float Kp, float Ki, float Kd)
{
int errorP;
@@ -138,86 +224,143 @@
return (float)Kp*errorP;
}
-int case_gerak(){
-/****************************************************
- ** Gerak Motor Base
- ** Case 1 : Pivot kanan
- ** Case 2 : Pivot Kiri
- ** Case 3 : Kanan
- ** Case 4 : Kiri
- ** Case 5 : Break
- ****************************************************/
-
- int casegerak;
- if (!joystick.L1 && joystick.R1) {
- // Pivot Kanan
- casegerak = 1;
- }
- else if (!joystick.R1 && joystick.L1) {
- // Pivot Kiri
- casegerak = 2;
- }
- else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
- // Kanan
- casegerak = 3;
-// pc.printf("kanan");
- }
- else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
- // Kiri
- casegerak = 4;
-// pc.printf("kiri");
- }
- else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
- // Break
- casegerak = 5;
+void init_speed (){
+ if (speed>maxSpeed){
+ speed = maxSpeed;
+ }
+ if (speed<minSpeed){
+ speed = minSpeed;
}
- return(casegerak);
+ if (speed2>maxSpeed){
+ speed2 = maxSpeed;
+ }
+ if (speed2<minSpeed){
+ speed2 = minSpeed;
+ }
+}
+
+void setCenter(){
+// Fungsi untuk menentukan center dari robot
+ encoderBase.reset();
}
-void aktuator(){
-/* Fungsi untuk menggerakkan Motor PowerScrew */
- // PowerScrew
- if (reload){
- if (!limitA){
- motorP.brake (1);
- if (hentis != henti){
- wait_ms(1500);
- }
- hentis = henti;
+void aktuator()
+{
+ switch (case_joy) {
+ case (1):
+ {
+ // Pivot Kanan
+ motorDpn.speed(-PIVOT);
+ motorBlk.speed(-PIVOT);
+ setCenter();
+ break;
+ }
+ case (2):
+ {
+ // Pivot Kiri
+ motorDpn.speed(PIVOT);
+ motorBlk.speed(PIVOT);
+ setCenter();
+ break;
+ }
+ case (3) :
+ {
+ // Kanan
+ motorDpn.speed(-(tuneDpn) + pidBase(0.09,0,0));
+ motorBlk.speed((tuneBlk) + pidBase(0.09,0,0));
+ break;
+ }
+ case (4) :
+ {
+ // Kiri
+ motorDpn.speed(tuneDpn + pidBase(0.09,0,0));
+ motorBlk.speed(-tuneBlk + pidBase(0.09,0,0));
+ break;
+ }
+ case (5) :
+ {
+ // Aktifkan motor atas
+ isLauncher = !isLauncher;
+ break;
}
- else
+ case (6) :
+ {
+ // Target Pulse PID ++ Motor Depan
+ target_rpm2 = target_rpm2++;
+ break;
+ }
+ case (7) :
+ {
+ // Target Pulse PID -- Motor Depan
+ target_rpm2 = target_rpm2--;
+ break;
+ }
+ case (8) :
+ {
+ // Target Pulse PID ++ Motor Belakang
+ target_rpm = target_rpm++;
+ break;
+ }
+ case (9) :
{
- henti = false;
- if (!limitT)
+ // Target Pulse PID -- Motor Belakang
+ target_rpm = target_rpm--;
+ break;
+ }
+ case (10) :
+ {
+ // Pneumatik
+ pneumatik = 0;
+ previousMillis3 = millis();
+ flag_Pneu = true;
+ break;
+ }
+ case (11) :
+ {
+ // Power Screw Up
+ powerScrew.speed(pwmPowerUp);
+ break;
+ }
+ case (12) :
+ {
+ // Power Screw Down
+ powerScrew.speed(pwmPowerDown);
+ break;
+ }
+ default :
+ {
+ motorDpn.brake(1);
+ motorBlk.brake(1);
+ if (!limitAtas)
{
- while (limitB)
- {
- motorP.speed(pwmT);
- }
- motorP.brake(1);
- reload = !reload;
- }
- else
+ powerScrew.brake(1);
+ }
+ if (!limitTengah)
+ {
+ case_joy = 12;
+ aktuator();
+ }
+ if (!limitBawah)
{
- motorP.speed(pwmP);
+ case_joy = 11;
+ aktuator();
}
- }
- }
- else
+ }
+ } // End Switch
+ }
+
+void launcher()
+{
+ if (isLauncher)
{
- motorP.brake (1);
- }
-
-/*Motor Atas*/
- if (launcher) {
startMillis();
currentMillis = millis();
currentMillis2 = millis();
- /*PID Launcher Depan*/
+ // PID Launcher Depan
if (currentMillis-previousMillis>=10)
{
- rpm = (float)encoderAtas.getPulses();
+ rpm = (float)encLauncherBlk.getPulses();
current_error = target_rpm - rpm;
sum_error = sum_error + current_error;
p = current_error*kpA;
@@ -225,19 +368,15 @@
i = sum_error*kiA*10.0;
speed = p + d + i;
init_speed();
- motorL1.speed(speed);
+ launcherBlk.speed(speed);
last_error = current_error;
- encoderAtas.reset();
+ encLauncherBlk.reset();
//pc.printf("%.04lf\n",rpm);
previousMillis = currentMillis;
}
- else
- {
- encoderAtas.getPulses();
- }
if (currentMillis2-previousMillis2>=10)
{
- rpm2 = (float)encoderAtas2.getPulses();
+ rpm2 = (float)encLauncherDpn.getPulses();
current_error2 = target_rpm2 - rpm2;
sum_error2 = sum_error2 + current_error2;
p2 = current_error2*kpA;
@@ -245,155 +384,54 @@
i2 = sum_error2*kiA*10.0;
speed2 = p2 + d2 + i2;
init_speed();
- motorL3.speed(speed2);
+ launcherDpn.speed(speed2);
last_error2 = current_error2;
- encoderAtas2.reset();
- //pc.printf("%.04lf\n",rpm2);
+ encLauncherDpn.reset();
previousMillis2 = currentMillis2;
}
- else
- {
- encoderAtas2.getPulses();
- }
}
else
{
- motorL1.brake(1);
- motorL3.brake(1);
- }
-
-
- // MOTOR Bawah
- switch (case_ger) {
- case (1): {
- // Pivot Kanan
- motor1.speed(PIVOT);
- motor2.speed(PIVOT);
- setCenter();
- break;
- }
- case (2): {
- // Pivot Kiri
- motor1.speed(-PIVOT);
- motor2.speed(-PIVOT);
- setCenter();
- break;
- }
- case (3) : {
- // Kanan
- //motor1.speed(-VMAX-vpid);
- //motor2.speed(0.2+vpid);
- motor1.speed(-0.365+pidBase(0.09,0,0));
- motor2.speed(0.46+pidBase(0.09,0,0));
- break;
- }
- case (4) : {
- // Kiri
- //motor1.speed(VMAX-vpid);
- //motor2.speed(-0.2+vpid);
- motor1.speed(0.365+pidBase(0.09,0,0));//belakang
- motor2.speed(-0.46+pidBase(0.09,0,0));//depan
- break;
- }
- default : {
- motor1.brake(1);
- motor2.brake(1);
- }
- } // End Switch
+ launcherDpn.brake(1);
+ launcherBlk.brake(1);
+ }
}
-
-void setCenter(){
-/* Fungsi untuk menentukan center dari robot */
- encoderKiri.reset();
-}
-
-float getTetha(){
-/* Fungsi untuk mendapatkan nilai tetha */
- float busurKir, tetha;
- busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc);
- tetha = busurKir/k_robot*360;
-
- return -(tetha);
-}
-
-void speedKalibrasiMotor(){
-/* Fungsi untuk speed launcher */
- if (joystick.R3_click and target_rpm < 14){
- target_rpm = target_rpm + 1; // RPM++ Motor Belakang
- }
- if (joystick.L3_click and target_rpm > 1){
- target_rpm = target_rpm - 1; // RPM-- Motor Belakang
- }
- if (joystick.R2_click and target_rpm2 < 14){
- target_rpm2 = target_rpm2 + 1; // RPM++ Motor Depan
- }
- if (joystick.L2_click and target_rpm2 > 1 ){
- target_rpm2 = target_rpm2 - 1; // RPM-- Motor Depan
- }
- // pc.printf("Rpm depan = %.4f\t Rpm belakang = %.4f\n", target_rpm, target_rpm2);
-}
-
-void init_speed (){
- if (speed>maxSpeed){
- speed = maxSpeed;
- }
-
- if (speed<minSpeed){
- speed = minSpeed;
- }
- if (speed2>maxSpeed){
- speed2 = maxSpeed;
- }
-
- if (speed2<minSpeed){
- speed2 = minSpeed;
- }
-}
+
/*********************************************************/
/* Main Function */
/*********************************************************/
-int main (void){
- /* Set baud rate - 115200 */
+int main (void)
+{
+ // Set baud rate - 115200
joystick.setup();
pc.baud(115200);
wait_ms(1000);
setCenter();
wait_ms(500);
pc.printf("ready....");
-
- /* Untuk mendapatkan serial dari Arduino */
while(1)
{
// Interrupt Serial
joystick.idle();
- //pc.printf("enco : %d \n",encoderKiri.getPulses());
- if(joystick.readable()) {
+ if(joystick.readable())
+ {
// Panggil fungsi pembacaan joystik
joystick.baca_data();
// Panggil fungsi pengolahan data joystik
joystick.olah_data();
- // Masuk ke case gerak
- case_ger = case_gerak();
+ // Masuk ke case joystick
+ case_joy = case_joystick();
aktuator();
- if (joystick.segitiga_click){
- launcher = !launcher;
- }
- if (joystick.lingkaran_click){
- pneumatik = 0;
- henti = true;
- wait_ms(700);
+ launcher();
+ if ((millis()-previousMillis3 >= 700)&&(flag_Pneu)){
pneumatik = 1;
+ flag_Pneu = false;
}
- speedKalibrasiMotor();
- if (joystick.silang_click){
- reload = !reload;
- }
}
- else {
- joystick.idle();
- motor1.brake(1);
- motor2.brake(1);
+ else
+ {
+ joystick.idle();
}
}
}
\ No newline at end of file