KRAI 2017
/
MainProgram_BaseBaru_display
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 Joystick_OrdoV5 mbed millis
Fork of
MainProgram_BaseBaru
by 
KRAI 2017
main.cpp
- Committer:
- Joshua23
- Date:
- 2017-02-10
- Revision:
- 25:054d3048dd03
- Parent:
- 24:b3e632cc4533
- Child:
- 26:256160a1a82d
File content as of revision 25:054d3048dd03:
/****************************************************************************/
/* PROGRAM UNTUK PID CLOSED LOOP */
/* */
/* - Digunakan encoder autonics */
/* - Konfigurasi Motor dan Encoder sbb : */
/* ______________________ */
/* / \ Rode Depan Belakang: */
/* / 2 (Belakang) \ Omniwheel */
/* | | */
/* | 3 (Encoder) 4 | Roda Kiri Kanan: */
/* | | Fixed Wheel */
/* \ 1 (Depan) / */
/* \______________________/ Putaran berlawanan arah */
/* jarum jam positif */
/* SETTINGS (WAJIB!) : */
/* 1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h */
/* 2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder */
/* */
/****************************************************************************/
/* */
/* Joystick */
/* Kanan => Posisi target x ditambah 'perpindahan' */
/* Kiri => Posisi target x dikurang 'perpindahan' */
/* */
/* Tombol silang => Kembali keposisi Awal */
/* Tombol segitiga => Aktif motor Launcher */
/* Tombol lingkaran=> Aktif servo Launcher */
/* 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 */
/* */
/* Bismillahirahmanirrahim */
/* Jagalah Kebersihan Kodingan */
/****************************************************************************/
#include "mbed.h"
#include "JoystickPS3.h"
#include "Motor.h"
#include "Servo.h"
#include "encoderKRAI.h"
/***********************************************/
/* Konstanta dan Variabel */
/***********************************************/
#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
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 */
encoderKRAI encoderKiri(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
/* 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
/* Deklarasi Motor Launcher */
//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 */
//Servo servoS(PB_2);
//Servo servoB(PA_5);
/**
* posX dan posY berdasarkan arah robot
* encoder Depan & Belakang sejajar sumbu Y
* encoder Kanan & Kiri sejajar sumbu X
**/
/* Variabel Encoder */
float errT, Tetha; // Variabel yang didapatkan encoder
/* 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);
/* Variabel Stick */
char case_ger;
bool launcher = false, servoGo = false;
/****************************************************/
/* Deklarasi Fungsi dan Procedure */
/****************************************************/
float pid(float Kp, float Ki, float Kd)
{
int errorP;
errorP = getTetha();
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;
}
return(casegerak);
}
void aktuator(){
/* Fungsi untuk menggerakkan servo */
// Servo
/* if (servoGo){
servoS.position(20);
wait_ms(500);
servoS.position(-28);
wait_ms(500);
servoS.position(20);
wait_ms(500);
for (int i = -0; i<=70; i++){
servoB.position(i);
wait_ms(10);
}
wait_ms(500);
servoB.position(0);
servoGo = false;
}
else{
servoS.position(20);
servoB.position(0);
}
// Motor Atas
if (launcher) {
motorld.speed(speedL);
motorlb.speed(speedB);
}else{
motorld.speed(0);
motorlb.speed(0);
}
*/
// 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+pid(0.09,0,0));
motor2.speed(0.46+pid(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
break;
}
default : {
motor1.brake(1);
motor2.brake(1);
}
} // End Switch
}
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 speedL < 0.8){
speedL = speedL + 0.01; // PWM++ Motor Belakang
}
if (joystick.L3_click and speedL > 0.1){
speedL = speedL - 0.01; // PWM-- Motor Belakang
}
if (joystick.R2_click and speedB < 0.8 ){
speedB = speedB + 0.01; // PWM++ Motor Depan
}
if (joystick.L2_click and speedB > 0.1 ){
speedB = speedB - 0.01; // PWM-- Motor Depan
}
// pc.printf("Pwm depan = %.3f\t Pwm belakang = %.3f\n", speedL, speedB);
}
/*********************************************************/
/* Main Function */
/*********************************************************/
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() ) {
// 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();
}
else {
joystick.idle();
//motor1.brake(1);
//motor2.brake(1);
}
}
}