KRAI 2017
/
MainProgram_BaseBaru_fix_omni_5April_fix
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of
MainProgram_BaseBaru_fix_omni12Feb
by 
KRAI 2017
Revision 44:452c214d9cf5, committed 2017-04-05
- Comitter:
- Najib_irvani
- Date:
- Wed Apr 05 12:54:03 2017 +0000
- Parent:
- 43:3807a95aa284
- Commit message:
- 5 april ganti kecepatan power screw. Yang bener
Changed in this revision
| PS3Arduino.txt | Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/PS3Arduino.txt Sun Mar 12 06:56:53 2017 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,299 +0,0 @@
-/*********************************************************************************************/
-/** **/
-/** PROGRAM KOMUNIKASI DATA JOYSTIK PS3 **/
-/** **/
-/** Joystik PS3 -> Arduino -> STM32Nucleo **/
-/** **/
-/** Fanny Achmad Hindrarta **/
-/** EL'12 - 13212076 **/
-/** **/
-/** Last Update : 01 Februar1 2015, 20.30 **/
-/*********************************************************************************************/
-
-#include <PS3BT.h>
-#include <usbhub.h>
-//#include <PS3USB.h>
-#include "Arduino.h"
-// Satisfy IDE, which only needs to see the include statment in the ino.
-#ifdef dobogusinclude
-#include <spi4teensy3.h>
-#endif
-
-
-USB Usb;
-
-// Bluetooth
-// You can create the instance of the class in two ways
-BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
-//You can create the instance of the class in two ways
-PS3BT PS3(&Btd); // This will just create the instance
-//PS3BT PS3(&Btd, 0x00, 0x15, 0x83, 0x3D, 0x0A, 0x57); // This will also store the bluetooth address - this can be obtained from the dongle when running the sketch
-/*
-// USB
-// You can create the instance of the class in two ways
-PS3USB PS3(&Usb); // This will just create the instance
-//PS3USB PS3(&Btd, 0x00, 0x15, 0x83, 0x3D, 0x0A, 0x57); // This will also store the bluetooth address - this can be obtained from the dongle when running the sketch
-*/
-
-boolean printAngle;
-uint8_t state = 0;
-
-// Deklarasi tombol
-boolean kiri_click=0, kanan_click=0, atas_click=0, bawah_click=0;
-boolean segitiga_click=0, lingkaran_click=0, kotak_click=0, silang_click=0;
-boolean L1_click=0, R1_click=0, L3_click=0, R3_click=0;
-boolean START_click=0, SELECT_click=0, PS_click=0;
-
-// Deklarasi variabel tombol analog
-unsigned char LX, LY, RX, RY, aL2, aR2;
-
-// Deklarasi varibel data yang dikirim
-unsigned char button;
-unsigned char RL;
-unsigned int button_click;
-unsigned int RL_click;
-
-void setup_joystik()
-{
- while (!Serial) // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
- {
- Serial.write(0x88);
- Serial.write(0x09);
- }
- if (Usb.Init() == -1) {
- //Serial.print(F("\r\nOSC did not start"));
- while (1) //halt
- {
- Serial.write(0x88);
- Serial.write(0x09);
- }
- }
- //Serial.print(F("\r\nPS3 USB Library Started"));
-}
-
-/*********************************************************************************************/
-/** **/
-/** FUNGSI PENGIRIMAN DATA **/
-/** - Data yang akan dikirim adalah paket data 8-bit dengan urutan sebagai berikut **/
-/** |------|------|--------|----|--------------|----------|----|----|----|----|----|----| **/
-/** | 0x88 | 0x08 | button | RL | button_click | RL_click | R2 | L2 | RX | RY | LX | LY | **/
-/** |------|------|--------|----|--------------|----------|----|----|----|----|----|----| **/
-/** **/
-/*********************************************************************************************/
-
-void kirimdatajoystik()
-{
-
- Serial.write(0x88);
- Serial.write(0x08);
- Serial.write(button);
- Serial.write(RL);
- Serial.write(button_click);
- Serial.write(RL_click);
- Serial.write(aR2);
- Serial.write(aL2);
- Serial.write(RX);
- Serial.write(RY);
- Serial.write(LX);
- Serial.write(LY);
-
-// Debug
-
-/*
- Serial.print(millis());
- Serial.print("\t");
- Serial.print(button);
- Serial.print("\t");
- Serial.print(RL);
- Serial.print("\t");
- Serial.print(button_click);
- Serial.print("\t");
- Serial.print(RL_click);
- Serial.print("\t");
- Serial.print(aR2);
- Serial.print("\t");
- Serial.print(aL2);
- Serial.print("\t");
- Serial.print(RX);
- Serial.print("\t");
- Serial.print(RY);
- Serial.print("\t");
- Serial.print(LX);
- Serial.print("\t");
- Serial.println(LY);
-*/
-}
-
-/*****************************************************************************************/
-/** SETUP REGISTER dan INISIALISASI **/
-/** - Setup Joystik **/
-/** - Baud Rate Serial 115200, 8-bit, 1 stop, 0 parity **/
-/*****************************************************************************************/
-
-void setup() {
- Serial.begin(115200);
-
- setup_joystik();
-}
-
-
-/*****************************************************************************************/
-/** **/
-/** FUNGSI PEMBACAAN DATA JOYSTIK **/
-/** - Data dari Joystik dikirim melalui Bluetooth. **/
-/** Lihat Library "PS3BT.h" **/
-/** - Data tombol dari joystik adalah data 1-bit. Pengiriman data melalui USART **/
-/** adalah 8-bit. Agar pengiriman data efisien, maka data 1-bit digabungkan **/
-/** agar terbentuk data 8-bit **/
-/** - Penggabungan data segitiga, longkaran, silang, kotak, arah atas, arah kanan, **/
-/** arah bawah, arah kiri disimpan dalam variabel "button" **/
-/** - Urutan data pada variabel "button" dan "button_click" **/
-/** adalah sebagai berikut **/
-/** **/
-/** |------|-------|-------|------|-------|--------|-----------|----------| **/
-/** Bit Ke | 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0 | **/
-/** |------|-------|-------|------|-------|--------|-----------|----------| **/
-/** Data | kiri | bawah | kanan | atas | kotak | silang | lingkaran | segitiga | **/
-/** |------|-------|-------|------|-------|--------|-----------|----------| **/
-/** **/
-/** - Penggabungan data R1, R2, L1, L2, R3, L3, START, dan SELECT disimpan dalam **/
-/** variabel "RL" **/
-/** - Urutan data pada variabel "RL" dan "RL_click" adalah **/
-/** sebagai berikut **/
-/** **/
-/** |----|--------|-------|----|----|----|----| **/
-/** Bit Ke | 6 | 5 | 4 | 3 | 2 | 1 | 0 | **/
-/** |----|--------|-------|----|----|----|----| **/
-/** Data | PS | SELECT | START | L3 | L1 | R3 | R1 | **/
-/** |----|--------|-------|----|----|----|----| **/
-/** **/
-/*****************************************************************************************/
-
-void loop() {
- button = 0;
- RL = 0;
- button_click=0;
- RL_click=0;
- Usb.Task();
-
- // Pembacaan data joystik dilakukan jika PS3 tersambung ke Arduino
- if (PS3.PS3Connected || PS3.PS3NavigationConnected) {
- // Pembacaan dan penggabungan data segitiga, lingkaran, silang, kotak, atas, kanan, bawah, kiri
- // Data bernilai '1' jika tombol ditekan
- if(PS3.getButtonPress(TRIANGLE)){
- button = button + (0x1 << 0);
- }
- if(PS3.getButtonPress(CIRCLE)){
- button = button + (0x1 << 1);
- }
- if(PS3.getButtonPress(CROSS)){
- button = button + (0x1 << 2);
- }
- if(PS3.getButtonPress(SQUARE)){
- button = button + (0x1 << 3);
- }
- if(PS3.getButtonPress(UP)){
- button = button + (0x1 << 4);
- }
- if(PS3.getButtonPress(RIGHT)){
- button = button + (0x1 << 5);
- }
- if(PS3.getButtonPress(DOWN)){
- button = button + (0x1 << 6);
- }
- if(PS3.getButtonPress(LEFT)){
- button = button + (0x1 << 7);
- }
-
- // Pembacaan dan penggabungan data R1, R3, L1, L3, START, SELECT, dan PS
- // Data bernilai '1' jika tombol ditekan
- if(PS3.getButtonPress(R1)){
- RL = RL + (0x1 << 0);
- }
- if(PS3.getButtonPress(R3)){
- RL = RL + (0x1 << 1);
- }
- if(PS3.getButtonPress(L1)){
- RL = RL + (0x1 << 2);
- }
- if(PS3.getButtonPress(L3)){
- RL = RL + (0x1 << 3);
- }
- if(PS3.getButtonPress(START)){
- RL = RL + (0x1 << 4);
- }
- if(PS3.getButtonPress(SELECT)){
- RL = RL + (0x1 << 5);
- }
- if(PS3.getButtonPress(PS)){
- RL = RL + (0x1 << 6);
- }
-
- // Pembacaan dan penggabungan data segitiga, lingkaran, silang, kotak, atas, kanan, bawah, kiri
- // Data bernilai '1' hanya saat tombol pertama kali ditekan
- if(PS3.getButtonClick(TRIANGLE)){
- button_click = button_click + (0x1 << 0);
- }
- if(PS3.getButtonClick(CIRCLE)){
- button_click = button_click + (0x1 << 1);
- }
- if(PS3.getButtonClick(CROSS)){
- button_click = button_click + (0x1 << 2);
- }
- if(PS3.getButtonClick(SQUARE)){
- button_click = button_click + (0x1 << 3);
- }
- if(PS3.getButtonClick(UP)){
- button_click = button_click + (0x1 << 4);
- }
- if(PS3.getButtonClick(RIGHT)){
- button_click = button_click + (0x1 << 5);
- }
- if(PS3.getButtonClick(DOWN)){
- button_click = button_click + (0x1 << 6);
- }
- if(PS3.getButtonClick(LEFT)){
- button_click = button_click + (0x1 << 7);
- }
-
- if(PS3.getButtonClick(R1)){
- RL_click = RL_click + (0x1 << 0);
- }
- if(PS3.getButtonClick(R3)){
- RL_click = RL_click + (0x1 << 1);
- }
- if(PS3.getButtonClick(L1)){
- RL_click = RL_click + (0x1 << 2);
- }
- if(PS3.getButtonClick(L3)){
- RL_click = RL_click + (0x1 << 3);
- }
- if(PS3.getButtonClick(START)){
- RL_click = RL_click + (0x1 << 4);
- }
- if(PS3.getButtonClick(SELECT)){
- RL_click = RL_click + (0x1 << 5);
- }
- if(PS3.getButtonClick(PS)){
- RL_click = RL_click + (0x1 << 6);
- }
-
- // Tombol Analog
- LX = PS3.getAnalogHat(LeftHatX);
- LY = PS3.getAnalogHat(LeftHatY);
- RX = PS3.getAnalogHat(RightHatX);
- RY = PS3.getAnalogHat(RightHatY);
-
- aL2 = PS3.getAnalogButton(L2);
- aR2 = PS3.getAnalogButton(R2);
-
- kirimdatajoystik();
- } else {
- // PS3 Disconnected
- Serial.write(0x88);
- Serial.write(0x09);
- }
- delay(3);
-}
-
-
\ No newline at end of file
--- a/main.cpp Sun Mar 12 06:56:53 2017 +0000
+++ b/main.cpp Wed Apr 05 12:54:03 2017 +0000
@@ -25,15 +25,14 @@
/* Kanan => */
/* Kiri => */
/* */
-/* Tombol silang => Power Screw Aktif */
+/* Tombol silang => Pneumatik aktif */
/* Tombol segitiga => Aktif motor Launcher */
-/* Tombol lingkaran => Aktif Pneumatik Launcher */
+/* Tombol lingkaran => Reloader naik */
+/* Tombol kotak => Reloader turun */
/* Tombol L1 => Pivot Kiri */
/* Tombol R1 => Pivot Kanan */
-/* Tombol L3 => PWM Motor Belakang Dikurangi */
-/* Tombol R3 => PWM Motor Belakang Ditambah */
-/* Tombol L2 => PWM Motor Depan Dikurangi */
-/* Tombol R2 => PWM Motor Depan Ditambah */
+/* Tombol L2 => Kurang PWM Motor Launcher */
+/* Tombol R2 => Tambah PWM Motor Launcher */
/* */
/* Bismillahirahmanirrahim */
/* Jagalah Kebersihan Kodingan */
@@ -53,22 +52,21 @@
#define PI 3.14159265
#define D_ENCODER 10 // Diameter Roda Encoder
#define D_ROBOT 80 // Diameter Roda Robot
-#define PERPINDAHAN 1 // Perpindahan ke kanan dan kiri
// Variable Atas
double speed, speed2;
const double maxSpeed = 0.95, minSpeed = 0.0;
-const double kpA=0.6757, kdA=0.06757, kiA=0.00006757;
+const double kpA=0.6757, kdA=0.7757, kiA=0.00003757;
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, rpm2;
-float target_rpm = 8.0, target_rpm2 = 10.0;
+float target_rpm = 17.0, target_rpm2 = 17.0; // selisih 4 bagus, sama bagus
const float maxRPM = 28, minRPM = 0; // Limit 25 atau 27
-const float pwmPowerUp = 0.8;
-const float pwmPowerDown = -0.9;
+const float pwmPowerUp = 1.0;
+const float pwmPowerDown = -1.0;
float jarak_ping=0;
@@ -77,13 +75,12 @@
float keliling_enc = PI*D_ENCODER;
float keliling_robot = PI*D_ROBOT;
float speedT = 0.2;
-float vpid = 0;
-float PIVOT = 0.2; // PWM Pivot Kanan, Pivot Kiri
-float tuneDpn = 0.62; // Tunning PWM motor Depan
-float tuneBlk = 0.62; // Tunning PWM motor belakang
-float tuneR = 0.72;
+float PIVOT = 0.17; // PWM Pivot Kanan, Pivot Kiri
+float tuneDpn = 0.80; // Tunning PWM motor Depan
+float tuneBlk = 0.80; // Tunning PWM motor belakang
+float tuneR = 0.78;
float tuneL = 0.72;
-float serong = 0.65;
+float serong = 0.4;
float rasio = 1.4545;
/* variable tunning */
@@ -111,6 +108,7 @@
bool isReload = false;
bool ReloadOn = false;
bool flag_Pneu = false;
+bool ready = false;
/*****************************************************/
/* Definisi Prosedur, Fungsi dan Setting Pinout */
@@ -132,33 +130,35 @@
encoderKRAI encoderBase(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), 2xresolusi, mode pembacaan
/* Deklarasi Encoder Launcher */
-encoderKRAI encLauncherBlk( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING);
-encoderKRAI encLauncherDpn( PD_2, PC_12, 28, encoderKRAI::X4_ENCODING);
+encoderKRAI encLauncherDpn( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING);
+encoderKRAI encLauncherBlk( PC_12, PD_2, 28, encoderKRAI::X4_ENCODING);
/* Deklarasi Motor Base */
-Motor motorDpn(PB_9, PA_12, PC_5); // pwm, fwd, rev
-Motor motorBlk(PB_6, PB_1, PB_12);
-Motor motorL (PA_11, PA_6, PA_7);
-Motor motorR (PB_7, PA_14, PA_15);
+Motor motorDpn(PB_7, PC_3, PC_0); //(PB_9, PA_12, PC_5);
+//Motor motorBlk(PB_6, PC_14, PC_13); //(PB_6, PB_1, PB_12); (PC_7, PC_14, PC_13);
+Motor motorBlk(PB_2, PB_15, PB_1);
+Motor motorL (PB_9, PA_12, PA_6);
+Motor motorR (PB_8, PC_5, PC_6); //(PC_6, PB_4, PB_5);
/* Deklarasi Motor Launcher */
-Motor launcherDpn(PA_8,PC_2,PC_1); // pwm ,fwd, rev
-Motor launcherBlk(PA_10, PC_3, PC_0); // pwm, fwd, rev
-Motor powerScrew(PA_9, PA_4, PC_15); // pwm, fwd, rev
+Motor launcherDpn(PA_5,PB_12,PA_11); // pwm ,fwd, rev
+Motor launcherBlk(PB_3, PC_4, PA_10); // pwm, fwd, rev
+Motor powerScrew(PB_10, PB_14, PB_13); // pwm, fwd, rev
/* Deklarasi Penumatik Launcher */
-DigitalOut pneumatik(PB_3, PullUp);
+DigitalOut pneumatik(PA_4, PullUp);
/*Dekalrasi Limit Switch */
//DigitalIn infraAtas(PC_9, PullUp);
-DigitalIn limitTengah(PB_10, PullUp);
-DigitalIn limitBawah(PC_8, PullUp);
+DigitalIn limitTengah(PA_9, PullUp);
+DigitalIn limitBawah(PC_7, PullUp);
+DigitalIn limitBawah1(PA_7, PullUp);
/*deklarasi PING ultrasonic*/
-Ping pingAtas(PC_9);
+Ping pingAtas(PC_15);
/*Deklarasi Display*/
-DigitDisplay display (D15, D4);
+DigitDisplay display (PA_8, PC_8);
/****************************************************/
/* Deklarasi Fungsi dan Procedure */
@@ -183,6 +183,18 @@
// Pivot Kiri
caseJoystick = 2;
}
+ else if ((joystick.START_click)&&(!joystick.SELECT_click)&&(!joystick.R3_click)) {
+ // tambah rpm dengan nilai tertentu
+ caseJoystick = 31;
+ }
+ else if ((!joystick.START_click)&&(joystick.SELECT_click)&&(!joystick.R3_click)) {
+ // kurangi rpm dengan nilai tertentu
+ caseJoystick = 32;
+ }
+ else if ((!joystick.START_click)&&(!joystick.SELECT_click)&&(joystick.R3_click)) {
+ // kurangi rpm dengan nilai tertentu
+ caseJoystick = 33;
+ }
else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
// Kanan + Rotasi kanan
caseJoystick = 17;
@@ -241,43 +253,35 @@
}
else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
// Serong kanan maju
- caseJoystick = 13;
- //pc.printf("bawah");
+ caseJoystick = 13;
}
else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
// Serong kiri maju
- caseJoystick = 14;
- //pc.printf("bawah");
+ caseJoystick = 14;
}
else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
// Serong kanan mundur
- caseJoystick = 15;
- //pc.printf("bawah");
+ caseJoystick = 15;
}
else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
// Serong kiri mundur
- caseJoystick = 16;
- //pc.printf("bawah");
+ caseJoystick = 16;
}
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.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
// Atas -- Maju
- caseJoystick = 8;
- //pc.printf("atas");
+ caseJoystick = 8;
}
else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
// Bawah -- Mundur
- caseJoystick = 9;
- //pc.printf("bawah");
+ caseJoystick = 9;
}
else if (joystick.segitiga_click){
// Motor Launcher
@@ -482,35 +486,36 @@
{
// Serong kanan maju
motorDpn.speed(-serong);
- motorL.brake(-rasio*serong);
+ motorL.speed(-serong);
motorBlk.speed(serong);
- motorR.brake(rasio*serong);
+ motorR.speed(serong);
break;
}
case (14) :
{
// Serong kiri maju
motorDpn.speed(serong);
- motorR.brake(rasio*serong);
+ motorR.speed(serong);
motorBlk.speed(-serong);
- motorL.brake(-rasio*serong);
+ motorL.speed(-serong);
break;
}
case (15) :
{
// Serong kanan mundur
- motorR.brake(-rasio*serong);
+ motorDpn.speed(-serong);
+ motorR.speed(-serong);
motorBlk.speed(serong);
- motorL.brake(rasio*serong);
+ motorL.speed(serong);
break;
}
case (16) :
{
// Serong kiri mundur
motorDpn.speed(serong);
- motorL.brake(rasio*serong);
+ motorL.speed(serong);
motorBlk.speed(-serong);
- motorR.brake(-rasio*serong);
+ motorR.speed(-serong);
break;
}
case (3) :
@@ -534,7 +539,6 @@
case (8) :
{
// Maju
- //init_rpm();
motorR.speed(tuneR);
motorL.speed(-tuneL);
motorDpn.brake(1);
@@ -544,7 +548,6 @@
case (9) :
{
// Mundur
- //init_rpm();
motorR.speed(-tuneR);
motorL.speed(tuneL);
motorDpn.brake(1);
@@ -576,9 +579,13 @@
case (10) :
{
// Pneumatik
- pneumatik = 0;
- previousMillis3 = millis();
- flag_Pneu = true;
+ if (ready)
+ {
+ pneumatik = 0;
+ previousMillis3 = millis();
+ flag_Pneu = true;
+ ready = false;
+ }
break;
}
case (11) :
@@ -588,6 +595,30 @@
isReload = false;
break;
}
+ case (31) :
+ {
+ // start
+ target_rpm2 = 22;
+ target_rpm = 22;
+ init_rpm();
+ break;
+ }
+ case (32) :
+ {
+ // select
+ target_rpm2 = 10;
+ target_rpm = 10;
+ init_rpm();
+ break;
+ }
+ case (33) :
+ {
+ // R3
+ target_rpm2 = 17;
+ target_rpm = 17;
+ init_rpm();
+ break;
+ }
case (12) :
{
// Power Screw Down
@@ -610,7 +641,7 @@
if(ReloadOn){
if(isReload){
powerScrew.speed(pwmPowerDown);
- if(!limitBawah){
+ if((!limitBawah)||(!limitBawah1)){
isReload = false;
ReloadOn = false;
}
@@ -620,12 +651,15 @@
}
else if((jarak_ping > 6.5) && !flag_Pneu){
powerScrew.speed(pwmPowerUp);
+ ready = false;
}
- else if((jarak_ping < 6 ) && !flag_Pneu) {
- powerScrew.speed(-0.1);
+ else if((jarak_ping < 6.0) && !flag_Pneu) {
+ powerScrew.speed(-0.85);
+ ready = false;
}
else{
powerScrew.brake(1);
+ ready = true;
}
}
else{
@@ -646,10 +680,10 @@
{
rpm = (float)encLauncherBlk.getPulses();
current_error = target_rpm - rpm;
- sum_error = sum_error + current_error;
+ sum_error = sum_error + current_error*12.5;
p = current_error*kpA;
d = (current_error-last_error)*kdA/12.5;
- i = sum_error*kiA*12.5;
+ i = sum_error*kiA;
speed = p + d + i;
//init_speed();
if(speed > maxSpeed){
@@ -670,10 +704,10 @@
{
rpm2 = (float)encLauncherDpn.getPulses();
current_error2 = target_rpm2 - rpm2;
- sum_error2 = sum_error2 + current_error2;
+ sum_error2 = sum_error2 + current_error2*12.5;
p2 = current_error2*kpA;
d2 = (current_error2-last_error2)*kdA/12.5;
- i2 = sum_error2*kiA*12.5;
+ i2 = sum_error2*kiA;
speed2 = p2 + d2 + i2;
//init_speed();
if (speed2 > maxSpeed){
@@ -689,11 +723,18 @@
encLauncherDpn.reset();
previousMillis2 = currentMillis2;
}
+ pc.printf("%.2f\t%.2f\n",rpm,rpm2);
}
else
{
launcherDpn.brake(1);
launcherBlk.brake(1);
+ sum_error = 0;
+ sum_error2 = 0;
+ current_error = 0;
+ current_error2 = 0;
+ last_error = 0;
+ last_error2 = 0;
}
}
@@ -740,7 +781,7 @@
joystick.olah_data();
// Masuk ke case joystick
case_joy = case_joystick();
- pc.printf("%d\n",case_joy);
+ //pc.printf("%d\n",case_joy);
aktuator();
launcher();
reloader();
@@ -754,13 +795,8 @@
joystick.idle();
}
- if(millis() - previousMillis5 >= 400){
- //display.write(0,((int) target_rpm2-2) / 10);
- //display.write(1,((int)target_rpm2-2) % 10);
- //display.write(2, (int)target_rpm2 / 10);
- //display.write(3, (int)target_rpm2 % 10);
- //display.setColon(true);
-
+ if(millis() - previousMillis5 >= 400)
+ {
display.write(0,((int) rpm2) / 10);
display.write(1,((int)rpm2) % 10);
display.write(2, (int)target_rpm2 / 10);