![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
dear josh
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of DagonFly__RoadToJapan_15Mei by
Diff: main.cpp
- Revision:
- 26:256160a1a82d
- Parent:
- 25:054d3048dd03
- Child:
- 27:68efb1622985
--- a/main.cpp Fri Feb 10 10:29:45 2017 +0000 +++ b/main.cpp Fri Feb 10 13:22:02 2017 +0000 @@ -41,42 +41,54 @@ #include "Motor.h" #include "Servo.h" #include "encoderKRAI.h" +#include "millis.h" /***********************************************/ /* Konstanta dan Variabel */ /***********************************************/ #define PI 3.14159265 -#define D_ENCODER 10 // Diameter Roda Encoder -#define D_ROBOT 80 // Diameter Roda Robot +#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, maxSpeed = 0.8, minSpeed = -0.8; +double kpA=0.6757, kdA=0.6757, kiA=0.00006757; +double p,i,d; +double last_error = 0, current_error, sum_error = 0; +double rpm, target_rpm = 9.0; + +// Variable Bawah float Vt; - float k_enc = PI*D_ENCODER; float k_robot = PI*D_ROBOT; - float speedT = 0.2; float speedB = 0.43; float speedL = 0.4; - float vpid = 0; -float kpX = 0.5, kpY = 0.5, kp_tetha = 0.03; -/* Deklarasi encoder */ +/* Deklarasi Encoder Base */ encoderKRAI encoderKiri(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan +/* Deklarasi Encoder Launcher */ +encoderKRAI encoderAtas( PB_13, PB_14, 14, encoderKRAI::X2_ENCODING); + /* Deklarasi Motor Base */ -Motor motor1(PB_9, PC_5, PA_12); // pwm, fwd, rev, Motor Depan -Motor motor2(PB_6, PB_1, PB_12); // pwm, fwd, rev, Motor Belakang +Motor motor1(PB_9, PC_5, PA_12); // pwm, fwd, rev, Motor Depan +Motor motor2(PB_6, PB_1, PB_12); // pwm, fwd, rev, Motor Belakang /* Deklarasi Motor Launcher */ +Motor motor3(PA_8,PC_1,PC_2); // pwm ,fwd, rev, Motor Atas //Motor motorld(PA_8, PC_1, PC_2); // pwm, fwd, rev //Motor motorlb(PA_0, PA_4, PC_15 ); // pwm, fwd, rev -/* Deklarasi Servo Launcher */ +/* Deklarasi Penumatik Launcher */ +DigitalOut pneumatik(PB_2, 0); + +/* Deklarasi Servo */ //Servo servoS(PB_2); //Servo servoB(PA_5); @@ -97,20 +109,24 @@ joysticknucleo joystick(PA_0,PA_1); Serial pc(USBTX,USBRX); +/* Deklarasi Variable Milis */ +unsigned long int previousMillis = 0; +unsigned long int currentMillis; + /* Variabel Stick */ char case_ger; -bool launcher = false, servoGo = false; +bool launcher = false; +//bool pneumatikGo = false; /****************************************************/ /* Deklarasi Fungsi dan Procedure */ /****************************************************/ - -float pid(float Kp, float Ki, float Kd) +void init_speed(); +void speedKalibrasiMotor(); +float pidBase(float Kp, float Ki, float Kd) { int errorP; - errorP = getTetha(); - return (float)Kp*errorP; } @@ -151,9 +167,9 @@ } void aktuator(){ -/* Fungsi untuk menggerakkan servo */ - // Servo -/* if (servoGo){ +/* Fungsi untuk menggerakkan Penumatik */ + // Pneumatik +/* if (pneumatikGo){ servoS.position(20); wait_ms(500); servoS.position(-28); @@ -172,16 +188,32 @@ servoS.position(20); servoB.position(0); } - - // Motor Atas +*/ + //Motor Atas if (launcher) { - motorld.speed(speedL); - motorlb.speed(speedB); - }else{ - motorld.speed(0); - motorlb.speed(0); + startMillis(); + currentMillis = millis(); + if (currentMillis-previousMillis>=10) + { + rpm = (double)encoderAtas.getPulses(); + current_error = target_rpm - rpm; + sum_error = sum_error + current_error; + p = current_error*kpA; + d = (current_error-last_error)*kdA/10.0; + i = sum_error*kiA*10.0; + speed = p + d + i; + init_speed(); + motor3.speed(speed); + last_error = current_error; + encoderAtas.reset(); + //pc.printf("%.04lf\n",rpm); + previousMillis = currentMillis; + } + else + { + motor3.speed(0); + } } -*/ // MOTOR Bawah switch (case_ger) { case (1): { @@ -202,21 +234,22 @@ // Kanan //motor1.speed(-VMAX-vpid); //motor2.speed(0.2+vpid); - motor1.speed(-0.365+pid(0.09,0,0)); - motor2.speed(0.46+pid(0.09,0,0)); + 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+pid(0.09,0,0));//belakang - motor2.speed(-0.46+pid(0.09,0,0));//depan + 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); + break; } } // End Switch } @@ -251,7 +284,16 @@ } // pc.printf("Pwm depan = %.3f\t Pwm belakang = %.3f\n", speedL, speedB); } - + +void init_speed (){ + if (speed>maxSpeed){ + speed = maxSpeed; + } + + if (speed<minSpeed){ + speed = minSpeed; + } +} /*********************************************************/ /* Main Function */ /*********************************************************/ @@ -271,25 +313,28 @@ // 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(); aktuator(); - - if (joystick.segitiga_click) launcher = !launcher; - if (joystick.lingkaran_click) servoGo = true; - speedKalibrasiMotor(); - } + if (joystick.segitiga_click){ + launcher = !launcher; + } + if (joystick.lingkaran_click){ + pneumatik = 1; + wait_ms(500); + pneumatik = 0; + } + speedKalibrasiMotor(); + } else { joystick.idle(); - //motor1.brake(1); - //motor2.brake(1); + motor1.brake(1); + motor2.brake(1); } } }