Simulasi Mobile Robot Menggunakan C#

Project ini saya buat dengan menggunakan Visual C# 2010 Express.

Berikut adalah program dari video di atas.

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Runtime.InteropServices;
using System.IO;

namespace AGV_2
{
    public partial class Form1 : Form
    {
        public Form1()
        {
            InitializeComponent();
        }

        //System of Robot===============================================
        int  interval = 10
            ,pembagi_interval = 10 // 100/interval
            ,encoder_kanan
            ,encoder_kiri
            ,error_enc_max = 100
            ,error_enc_min = -100;
        //satu putaran 360', jarak encoder = 63
        static Random error = new Random();
        static int x = 300, y = 300, end_x, end_y;
        double  x_buff = (double)x
               ,y_buff=(double)y
               ,angle = 0
               ,speed_kanan_act, speed_kiri_act
               ,encoder_kanan_buff
               ,encoder_kiri_buff
               ,speed;
        //======================================System of Robot=======
        private void timer1_Tick(object sender, EventArgs e)
        {
            //System of Robot===============================================

            Graphics g = this.CreateGraphics();
            Pen p = new Pen(Color.Green, 3);
            Pen erase = new Pen(Color.White, 3);
            g.DrawEllipse(erase, x - 10, y - 10, 20, 20);
            g.DrawLine(erase, new Point(x, y), new Point(end_x, end_y));

            Update_AGV();
            encoder_kanan_buff += ((double)(speed_kanan_act )/ (double)interval);
            encoder_kanan = (int)encoder_kanan_buff;
            encoder_kiri_buff += ((double)(speed_kiri_act ) / (double)interval);
            encoder_kiri = (int)encoder_kiri_buff;

            label4.Text = encoder_kanan.ToString();
            label5.Text = encoder_kiri.ToString();
            label8.Text = speed_kanan_act.ToString();
            label9.Text = speed_kiri_act.ToString();

            speed = ((double)((speed_kanan_act / (double)interval) + (speed_kiri_act / (double)interval)) / 2);

            angle += (5.7272727272727272727272727272727 * ((double)((speed_kanan_act / (double)interval) - (speed_kiri_act / (double)interval)) / 2));

            x_buff = (x_buff + (speed * Math.Sin(angle * 0.0174532925)));
            y_buff = (y_buff + (speed * Math.Cos(angle * 0.0174532925)));

            x = (int)x_buff;
            y = (int)y_buff;

            if ((x >= 1279)) x = 1279;
            if ((y >= 750)) y = 750;
            if ((x <= 0)) x = 0;
            if ((y <= 0)) y = 0;

            end_x = (int)(x_buff + (15 * Math.Sin(angle * 0.0174532925)));
            end_y = (int)(y_buff + (15 * Math.Cos(angle * 0.0174532925)));

            label1.Location = new Point(x - 35, y + 30);
            label1.Text = String.Format("θ={0:00.0}x={1}y={2}", angle,x,y);

            g.DrawEllipse(p, x - 10, y - 10, 20, 20);
            g.DrawLine(p, new Point(x, y), new Point(end_x, end_y));
            //======================================System of Robot=======
        }

        /*
         * Keterangan:
         * -diameter robot 20 pixel
         * -speed robot adalah perpindahan robot pixel/cycle-execution
         * Input System:
         * 1.speed_kanan --> int
         * 2.speed_kiri --> int
         * Feedback System:
         * 1.encoder_kanan --> int
         * 2.encoder_kiri --> int
         */

        //Di bawah ini algoritma navigasi
        int theta
            ,X_position
            ,Y_position;
        double distance
              ,X_position_buff
              ,Y_position_buff
              ,theta_buff;
        void Update_AGV()
        {
            distance = ((double)(encoder_kiri + encoder_kanan) / 2);
            theta_buff += 5.7272727272727272727272727272727 * ((double)(encoder_kiri - encoder_kanan) / 2);
            theta = (int)theta_buff;

            X_position_buff += (int)(distance * Math.Sin(theta * 0.0174532925));
            Y_position_buff += (int)(distance * Math.Cos(theta * 0.0174532925));
            X_position = (int)X_position_buff;
            Y_position = (int)Y_position_buff;

            label_distance.Text = distance.ToString();
            label_theta.Text = theta.ToString();
            label_xpos.Text = X_position.ToString();
            label_ypos.Text = Y_position.ToString();

            speed_kanan_act = 9;
            speed_kiri_act = 10;

            /*
            Target +=(((float)Target-Math.Abs(Heading_Error))*0.045);
            Target_Bearing = 90.0 - (Rads*(atan(y/x)));

            Heading_error += target_bearing - (theta*RADS);
            Steering+=((float)Heading_Error*Kp)+((float)D_Error*Kd);  

            MKA= Target - Steering;  
            MKI= Target + Steering; */
        }

        private void Form1_Load(object sender, EventArgs e)
        {
            timer1.Interval = pembagi_interval*interval;
        }

    }
}

Berikut adalah file project di atas yang bisa didownload. http://www.mediafire.com/file/mswhcvx76cd7ww2/AGV_2_V1.0.rar

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s