PID omniwheel 3 roda

Dependencies:   Motor PS_PAD TextLCD mbed-os

Fork of cobaLCDJoyMotor_Thread by EL4121 Embedded System

Revision:
0:837acb06c892
Child:
1:2bf3dac65b08
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Nov 24 14:33:01 2015 +0000
@@ -0,0 +1,247 @@
+#include "mbed.h"
+#include "TextLCD.h"
+
+TextLCD lcd(PA_8, PB_10, PB_4, PB_5, PB_3, PA_10, TextLCD::LCD20x4); //rs,e,d4-d7,
+
+Serial pc(USBTX,USBRX);
+
+DigitalOut selector_a (PA_4);
+DigitalOut selector_b (PB_0);
+DigitalOut selector_c (PC_1);
+
+DigitalIn but0(PC_8);
+
+AnalogIn sensorkanan (PA_1);
+AnalogIn sensorkiri(PA_0);
+
+void Selektor (int output)
+{
+    if (output == 0)
+    {
+        selector_a = 0;
+        selector_b = 0;
+        selector_c = 0;
+    }
+    else if (output ==1)
+    {
+        selector_a = 1;
+        selector_b = 0;
+        selector_c = 0;
+    }
+    else if (output ==2)
+    {
+        selector_a = 0;
+        selector_b = 1;
+        selector_c = 0;
+    }
+    else if (output ==3)
+    {
+        selector_a = 1;
+        selector_b = 1;
+        selector_c = 0;
+    }
+    else if (output ==4)
+    {
+        selector_a = 0;
+        selector_b = 0;
+        selector_c = 1;
+    }
+    else if (output ==5)
+    {
+        selector_a = 1;
+        selector_b = 0;
+        selector_c = 1;
+    }
+    else if (output ==6)
+    {
+        selector_a = 0;
+        selector_b = 1;
+        selector_c = 1;
+    }
+    else if (output ==7)
+    {
+        selector_a = 1;
+        selector_b = 1;
+        selector_c = 1;
+    }
+}
+char sensor_ki (int urutan)
+{   
+    char x;
+    //int hasil; 
+    Selektor(urutan);
+    wait_ms(1);
+    if (sensorkiri.read() >= 1)
+    {
+        
+        x='1';
+    }
+    else
+    {
+        x='0';
+    }
+    
+    return x;
+}
+char sensor_ka (int urutan )
+{
+    char x;
+    //int hasil ;
+    Selektor(urutan);
+    wait_ms(1);
+    if (sensorkanan.read() >= 1)
+    {
+        x='1';
+    }
+    else
+    {
+        x='0';
+    }
+    
+    return x;
+}
+
+float sensor_ki_adc (int urutan)
+{   
+    //int hasil; 
+    Selektor(urutan);
+    wait_ms(1);
+    
+    return sensorkiri.read();
+}
+float sensor_ka_adc (int urutan )
+{
+    //int hasil ;
+    Selektor(urutan);
+    wait_ms(1);
+
+    return sensorkanan.read();
+}
+
+void tampil_lcd_bin()
+{
+    lcd.locate(2,3);
+      lcd.putc(sensor_ki(7));
+      lcd.locate(3,3);
+      //pc.printf ("      ");
+      lcd.putc(sensor_ki(6));
+      lcd.locate(4,3);
+      //pc.printf ("      ");
+      lcd.putc(sensor_ki(5));
+      lcd.locate(5,3);
+      lcd.putc(sensor_ki(4));
+      lcd.locate(6,3);
+      lcd.putc(sensor_ki(3));
+      lcd.locate(7,3);
+      lcd.putc(sensor_ki(2));
+      lcd.locate(8,3);
+      lcd.putc(sensor_ki(1));
+      lcd.locate(9,3);
+      lcd.putc(sensor_ki(0));
+      lcd.locate(10,3);
+      lcd.putc(sensor_ka(7));
+      lcd.locate(11,3);
+      lcd.putc(sensor_ka(6));
+      lcd.locate(12,3);
+      lcd.putc(sensor_ka(5));
+      lcd.locate(13,3);
+      lcd.putc(sensor_ka(4));
+      lcd.locate(14,3);
+      lcd.putc(sensor_ka(3));
+      lcd.locate(15,3);
+      lcd.putc(sensor_ka(2));
+      lcd.locate(16,3);
+      lcd.putc(sensor_ka(1));
+      lcd.locate(17,3);
+      lcd.putc(sensor_ka(0));
+      lcd.locate(18,3);
+}
+
+void tampil_lcd_adc()
+{
+    int k,j,i;
+    i=0;     
+       
+    while(i <= 3)
+    {
+       j=0;
+       k=7;
+       while(j<16 && k>3)
+       {           
+           lcd.locate(j,i);
+           if(i==0)
+           {
+                    //lcd.locate(j,i);
+              lcd.printf("%1.2f",sensor_ki_adc(k));
+           }
+           else if(i==1)
+           {
+                   //lcd.locate(j,i);
+              lcd.printf("%1.2f",sensor_ki_adc(k-4));
+           }
+           else if(i==2)
+           {
+                 //lcd.locate(j,i);
+              lcd.printf("%1.2f",sensor_ka_adc(k));
+           }
+           else if(i==3)
+           {
+                  //lcd.locate(j,i);
+              lcd.printf("%1.2f",sensor_ka_adc(k-4));
+           }
+           
+           j=j+5;
+           k--;
+        }    
+        i++;
+    }
+}
+
+    
+
+int main()
+{    
+    pc.baud(115200);
+    //pc.printf("Pembacaan ADC Sensor : \n");
+    while (1)
+    {      
+        if(but0  == 1)
+        {
+            tampil_lcd_adc(); 
+            //wait_ms(100);  
+        }
+        else
+        {
+            lcd.locate(0,0);
+            lcd.printf("rizqi cahyo Yuwono");
+            lcd.locate(0,1);
+            lcd.printf("12314090");
+            tampil_lcd_bin();  
+        } 
+        
+        pc.printf ("%f", sensor_ki_adc(7));
+      //pc.printf ("      ");
+      pc.printf (" %f ", sensor_ki_adc(6));
+      //pc.printf ("      ");
+      pc.printf (" %f ", sensor_ki_adc(5));
+      pc.printf (" %f ", sensor_ki_adc(4));
+      pc.printf (" %f ", sensor_ki_adc(3));
+      pc.printf (" %f ", sensor_ki_adc(2));
+      pc.printf (" %f ", sensor_ki_adc(1));
+      pc.printf (" %f ", sensor_ki_adc(0));
+      pc.printf ("\t\t");
+      pc.printf (" %f ", sensor_ka_adc(7));
+      pc.printf (" %f ", sensor_ka_adc(6));
+      pc.printf (" %f ", sensor_ka_adc(5));
+      pc.printf (" %f ", sensor_ka_adc(4));
+      pc.printf (" %f ", sensor_ka_adc(3));
+      pc.printf (" %f ", sensor_ka_adc(2));
+      pc.printf (" %f ", sensor_ka_adc(1));
+      pc.printf (" %f ", sensor_ka_adc(0));
+      pc.printf ("\r\n");
+    
+        wait_ms(10);
+        lcd.cls();
+    }
+}
+        
\ No newline at end of file