KRAI 2016
base versi 1 motherboard prototype
Dependencies: Motor NewTextLCD PID mbed
main.cpp
- Committer:
- rizqicahyo
- Date:
- 2015-12-19
- Revision:
- 2:23fe981b6ef7
- Parent:
- 1:33203568631b
File content as of revision 2:23fe981b6ef7:
/*********************************************************************************************/
/** GARUDAGO-ITB (KRAI2016) **/
/** #ROADTOBANGKOK! **/
/** **/
/** MAIN PROGRAM ROBOT HYBRID SEMI OTOMATIS **/
/** **/
/** **/
/** Created by : **/
/** Rizqi Cahyo Yuwono **/
/** EL'14 - 13214090 **/
/** **/
/** Last Update : 19 Desember 2015, 06.10 WIB **/
/*********************************************************************************************/
/*********************************************************************************************/
/** FILE HEADER **/
/*********************************************************************************************/
#include "mbed.h"
#include "Motor.h"
#include "NewTextLCD.h"
#include "PS3_USB.h"
#include "PID.h"
/*********************************************************************************************/
/** DEKLARASI INPUT OUTPUT **/
/*********************************************************************************************/
// serial pc
Serial pc(USBTX,USBRX);
// LCD 20x4
TextLCD lcd(PB_13, PB_14, PB_15, PB_1, PB_2, PB_5, TextLCD::LCD20x4); //rs,e,d4-d7
// joystick PS3
PS3_USB PS3 (PA_11,PA_12); //(rx,tx)
// PID sensor garis
PID PID(0.532,0.000,0.41,0.001); //(P,I,D, time sampling)
// Motor
Motor motor_kiri (PA_15, PA_14, PA_13); //motor1 (pwm, fwd, rev)
Motor motor_kanan (PA_1, PC_14 ,PC_15); //motor2 (pwm, fwd, rev)
// Selektor dan Sensor
BusOut selektor(PB_0,PA_9,PC_2); //(selektor A, selektor B, selektor C)
AnalogIn sensorR (PC_0); //ADC1
AnalogIn sensorL(PC_3); //ADC2
// Multitasker
Ticker timer;
/*********************************************************************************************/
/** DEKLARASI VARIABEL GLOBAL **/
/*********************************************************************************************/
float gLimit=0.4; //nilai batas sensor agar dapat membedakan warna putih dengan background
float gMax_speed=0.5; //nilai maksimum kecepatan motor
float gMin_speed=-0.05; //nilai minimum kecepatan motor
unsigned char gMode=0; //variabel mode driving (manual = 0 otomatis = 1)
unsigned char gCase=0; //variabel keadaan proses
unsigned char logic_sensor[16]; //variabel logic dari sensor garis
unsigned char over=0;
unsigned char i; // variabel iterasi
/*********************************************************************************************/
/** DEKLARASI PROSEDUR DAN FUNGSI **/
/*********************************************************************************************/
float adcSensor(int i) //pembacaan nilai ADC dari sensor garis
{
if (i < 8)
{
selektor = 7-i;
return sensorL.read();
}
else
{
selektor = 15-i;
return sensorR.read();
}
}
void PIDrunning() //menjalankan perintah untuk line follower
{
int pv;
float speedR,speedL;
//menentukan logic sensor
for(i=1;i<16;i++){
if(adcSensor(i) <= gLimit)
logic_sensor[i]=1;
else
logic_sensor[i]=0;
}
//////////////////logic dari PV (present Value)/////////////////////////
if(logic_sensor[0]==1){
pv = -15;
over=1;
}
else if(logic_sensor[15]==1){
pv = 15;
over=2;
}
else if(logic_sensor[1]==1 && logic_sensor[0]==1){
pv = -14;
}
else if(logic_sensor[14]==1 && logic_sensor[15]==1){
pv = 14;
}
else if(logic_sensor[1]==1 ){
pv = -13;
}
else if(logic_sensor[14]==1 ){
pv = 13;
}
else if(logic_sensor[2]==1 && logic_sensor[1]==1){
pv = -12;
}
else if(logic_sensor[13]==1 && logic_sensor[14]==1){
pv = 12;
}
else if(logic_sensor[2]==1 ){
pv = -11;
}
else if(logic_sensor[13]==1 ){
pv = 11;
}
else if(logic_sensor[3]==1 && logic_sensor[2]==1){
pv = -10;
}
else if(logic_sensor[12]==1 && logic_sensor[13]==1){
pv = 10;
}
else if(logic_sensor[3]==1 ){
pv = -9;
}
else if(logic_sensor[12]==1 ){
pv = 9;
}
else if(logic_sensor[4]==1 && logic_sensor[3]==1){
pv = -8;
}
else if(logic_sensor[11]==1 && logic_sensor[12]==1){
pv = 8;
}
else if(logic_sensor[4]==1 ){
pv = -7;
}
else if(logic_sensor[11]==1 ){
pv = 7;
}
else if(logic_sensor[5]==1 && logic_sensor[4]==1){
pv = -6;
}
else if(logic_sensor[10]==1 && logic_sensor[11]==1){
pv = 6;
}
else if(logic_sensor[5]==1 ){
pv = -5;
}
else if(logic_sensor[10]==1 ){
pv = 5;
}
else if(logic_sensor[6]==1 && logic_sensor[5]==1){
pv = -4;
}
else if(logic_sensor[9]==1 && logic_sensor[10]==1){
pv = 4;
}
else if(logic_sensor[6]==1 ){
pv = -3;
}
else if(logic_sensor[9]==1 ){
pv = 3;
}
else if(logic_sensor[7]==1 && logic_sensor[6]==1){
pv = -2;
}
else if(logic_sensor[8]==1 && logic_sensor[9]==1){
pv = 2;
}
else if (logic_sensor[7]==1 && logic_sensor[8]==1){
pv = 0;
}
else if(logic_sensor[7]==1 ){
pv = -1;
}
else if(logic_sensor[8]==1 ){
pv = 1;
}
///////////////// robot bergerak keluar dari sensor/////////////////////
if(over=1){
/*if(speed_ka > 0){
wait_ms(10);
motor_kanan.speed(-speed_ka);
wait_ms(10);
}
else{
motor_kanan.speed(speed_ka);
}
wait_ms(10);*/
motor_kiri.brake(1);
//wait_ms(100);
}
else if(over==2){
/*if(speed_ki > 0){
wait_ms(10);
motor_kiri.speed(-speed_ki);
wait_ms(10);
}
else{
wait_ms(10);
motor_kiri.speed(speed_ki);
wait_ms(10);
}
wait_ms(10);*/
motor_kanan.brake(1);
//wait_ms(100);
}
PID.setProcessValue(pv);
PID.setSetPoint(0);
// memulai perhitungan PID
speedR = gMax_speed + PID.compute();
if(speedR > gMax_speed){
speedR = gMax_speed;
}
else if(speedR < gMin_speed)
speedR = gMin_speed;
motor_kanan.speed(speedR);
speedL = gMax_speed - PID.compute();
if(speedL > gMax_speed)
speedL = gMax_speed;
else if(speedL < gMin_speed)
speedL = gMin_speed;
motor_kiri.speed(speedL);
over=0;
}
void showLCD() //menampilkan user interface pada LCD
{
lcd.cls();
lcd.locate(5,0);
lcd.printf("GarudaGo !!");
switch(gCase)
{
case 0 :
{
lcd.locate(1,1);
lcd.printf("Kalibrasi = %.4f",gLimit);
break;
}
case 1 :
{
if (gMode == 1)
{
lcd.locate(3,1);
lcd.printf("Mode = Otomatis");
}
else if (gMode==0)
{
lcd.locate(3,1);
lcd.printf("Mode = Manual");
}
break;
}
}
for(i=0;i<16;i++){
if(adcSensor(i) <= gLimit)
logic_sensor[i]=1;
else
logic_sensor[i]=0;
lcd.locate(i+2,3);
lcd.printf("%d",logic_sensor[i]);
}
}
void running() //prosedur utama untuk kendali robot
{
float speed=gMax_speed;
float k=1;
switch(gCase)
{
case 0 :
{
motor_kiri.brake(0);
motor_kanan.brake(0);
if(PS3.L2 == 0 && PS3.R2==255 && !PS3.R1 && !PS3.L1)
gLimit += 0.00001;
else if (PS3.L2 == 255 && PS3.R2==0 && !PS3.R1 && !PS3.L1)
gLimit -= 0.00001;
else if(PS3.L2 == 0 && PS3.R2==0 && PS3.R1 && !PS3.L1)
gLimit += 0.0005;
if(PS3.L2 == 0 && PS3.R2==0 && !PS3.R1 && PS3.L1)
gLimit -= 0.0005;
if (gLimit > 1.0)
gLimit = 1.0;
else if(gLimit < 0)
gLimit = 0.0;
if(PS3.START && PS3.L2 == 0 && PS3.R2 == 0 && !PS3.R1 && !PS3.L1)
gCase++;
break;
}
case 1 :
{
if (gMode == 1)
{
if (PS3.silang)
{
PIDrunning();
pc.printf("PID \t %f \t ",PID.compute());
for(i=0;i<16;i++)
{
pc.printf("%d ",logic_sensor[i]);
}
pc.printf("\n");
}
else if(!PS3.silang)
{
motor_kiri.brake(0);
motor_kanan.brake(0);
pc.printf("PID stop \n");
}
}
else if (gMode == 0)
{
if (PS3.atas && !PS3.bawah && !PS3.L1 && !PS3.R1) //maju
{
if (speed < 0.1)
speed = 0.1;
else
speed += 0.005;
if (speed >= gMax_speed)
speed = gMax_speed;
motor_kiri.speed(speed*k);
motor_kanan.speed(speed*k);
pc.printf("maju \n");
}
else if (!PS3.atas && PS3.bawah && !PS3.L1 && !PS3.R1) //mundur
{
if (speed < 0.1)
speed = 0.1;
else
speed += 0.005;
if (speed >= gMax_speed)
speed = gMax_speed;
motor_kiri.speed(-speed*k);
motor_kanan.speed(-speed*k);
pc.printf("mundur \n");
}
else if (!PS3.atas && !PS3.bawah && PS3.L1 && !PS3.R1) //pivot kiri
{
if (speed < 0.1)
speed = 0.1;
else
speed += 0.005;
if (speed >= gMax_speed)
speed = gMax_speed;
motor_kiri.speed(-speed*k*0.7);
motor_kanan.speed(speed*k*0.7);
pc.printf("piv kiri \n");
}
else if (!PS3.atas && !PS3.bawah && !PS3.L1 && PS3.R1) //pivot kanan
{
if (speed < 0.1)
speed = 0.1;
else
speed += 0.005;
if (speed >= gMax_speed)
speed = gMax_speed;
motor_kiri.speed(speed*k*0.7);
motor_kanan.speed(-speed*k*0.7);
pc.printf("piv kanan \n");
}
else if (!PS3.atas && !PS3.bawah && !PS3.L1 && !PS3.R1)
{
motor_kiri.brake(0);
motor_kanan.brake(0);
pc.printf("stop \n");
}
/*if(PS3.L2 == 255 && PS3.R2 == 0)
k=0.5;
else if (PS3.L2 == 0 && PS3.R2 == 255)
k=2;
else
k=1;*/
}
else
{
motor_kiri.brake(1);
motor_kanan.brake(1);
pc.printf("no PID no MANUAL \n");
}
if(PS3.SELECT && !PS3.START)
{
if (gMode==1)
gMode=0;
else
gMode=1;
}
if(PS3.START && !PS3.SELECT)
gCase--;
break;
}
}
}
/*********************************************************************************************/
/*********************************************************************************************/
/** PROGRAM UTAMA **/
/*********************************************************************************************/
/*********************************************************************************************/
int main(void){
//inisialisasi joystick
PS3.setup();
//inisialisasi PID
PID.setInputLimits(-15,15);
PID.setOutputLimits(-1.0,1.0);
PID.setMode(1.0);
PID.setBias(0.0);
PID.reset();
// serial PC
pc.baud(115200);
//multitasking untuk menampilkan layar
timer.attach(&showLCD,1);
while(1){
PS3.idle();
if(PS3.readable())
{
// Panggil fungsi pembacaan joystik
PS3.baca_data();
// Panggil fungsi pengolahan data joystik
PS3.olah_data();
//pc.printf("%2x %2x %2x %2x %3d %3d %3d %3d %3d %3d\n\r",PS3.button, PS3.RL, PS3.button_click, PS3.RL_click, PS3.R2, PS3.L2, PS3.RX, PS3.RY, PS3.LX, PS3.LY);
running();
}
else
{
PS3.idle();
}
}
}