EL4121 Embedded System / Mbed OS odometry_omni_3roda_v3

PID + multiple point

Dependencies:   Motor PS_PAD TextLCD mbed-os

Fork of cobaLCDJoyMotor_Thread by EL4121 Embedded System

main.cpp@0:837acb06c892, 2015-11-24 (annotated)

Committer:
rizqicahyo
Date:
Tue Nov 24 14:33:01 2015 +0000
Revision:
0:837acb06c892
Child:
1:2bf3dac65b08
tugas LCD

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rizqicahyo 0:837acb06c892 1 #include "mbed.h"
rizqicahyo 0:837acb06c892 2 #include "TextLCD.h"
rizqicahyo 0:837acb06c892 3
rizqicahyo 0:837acb06c892 4 TextLCD lcd(PA_8, PB_10, PB_4, PB_5, PB_3, PA_10, TextLCD::LCD20x4); //rs,e,d4-d7,
rizqicahyo 0:837acb06c892 5
rizqicahyo 0:837acb06c892 6 Serial pc(USBTX,USBRX);
rizqicahyo 0:837acb06c892 7
rizqicahyo 0:837acb06c892 8 DigitalOut selector_a (PA_4);
rizqicahyo 0:837acb06c892 9 DigitalOut selector_b (PB_0);
rizqicahyo 0:837acb06c892 10 DigitalOut selector_c (PC_1);
rizqicahyo 0:837acb06c892 11
rizqicahyo 0:837acb06c892 12 DigitalIn but0(PC_8);
rizqicahyo 0:837acb06c892 13
rizqicahyo 0:837acb06c892 14 AnalogIn sensorkanan (PA_1);
rizqicahyo 0:837acb06c892 15 AnalogIn sensorkiri(PA_0);
rizqicahyo 0:837acb06c892 16
rizqicahyo 0:837acb06c892 17 void Selektor (int output)
rizqicahyo 0:837acb06c892 18 {
rizqicahyo 0:837acb06c892 19 if (output == 0)
rizqicahyo 0:837acb06c892 20 {
rizqicahyo 0:837acb06c892 21 selector_a = 0;
rizqicahyo 0:837acb06c892 22 selector_b = 0;
rizqicahyo 0:837acb06c892 23 selector_c = 0;
rizqicahyo 0:837acb06c892 24 }
rizqicahyo 0:837acb06c892 25 else if (output ==1)
rizqicahyo 0:837acb06c892 26 {
rizqicahyo 0:837acb06c892 27 selector_a = 1;
rizqicahyo 0:837acb06c892 28 selector_b = 0;
rizqicahyo 0:837acb06c892 29 selector_c = 0;
rizqicahyo 0:837acb06c892 30 }
rizqicahyo 0:837acb06c892 31 else if (output ==2)
rizqicahyo 0:837acb06c892 32 {
rizqicahyo 0:837acb06c892 33 selector_a = 0;
rizqicahyo 0:837acb06c892 34 selector_b = 1;
rizqicahyo 0:837acb06c892 35 selector_c = 0;
rizqicahyo 0:837acb06c892 36 }
rizqicahyo 0:837acb06c892 37 else if (output ==3)
rizqicahyo 0:837acb06c892 38 {
rizqicahyo 0:837acb06c892 39 selector_a = 1;
rizqicahyo 0:837acb06c892 40 selector_b = 1;
rizqicahyo 0:837acb06c892 41 selector_c = 0;
rizqicahyo 0:837acb06c892 42 }
rizqicahyo 0:837acb06c892 43 else if (output ==4)
rizqicahyo 0:837acb06c892 44 {
rizqicahyo 0:837acb06c892 45 selector_a = 0;
rizqicahyo 0:837acb06c892 46 selector_b = 0;
rizqicahyo 0:837acb06c892 47 selector_c = 1;
rizqicahyo 0:837acb06c892 48 }
rizqicahyo 0:837acb06c892 49 else if (output ==5)
rizqicahyo 0:837acb06c892 50 {
rizqicahyo 0:837acb06c892 51 selector_a = 1;
rizqicahyo 0:837acb06c892 52 selector_b = 0;
rizqicahyo 0:837acb06c892 53 selector_c = 1;
rizqicahyo 0:837acb06c892 54 }
rizqicahyo 0:837acb06c892 55 else if (output ==6)
rizqicahyo 0:837acb06c892 56 {
rizqicahyo 0:837acb06c892 57 selector_a = 0;
rizqicahyo 0:837acb06c892 58 selector_b = 1;
rizqicahyo 0:837acb06c892 59 selector_c = 1;
rizqicahyo 0:837acb06c892 60 }
rizqicahyo 0:837acb06c892 61 else if (output ==7)
rizqicahyo 0:837acb06c892 62 {
rizqicahyo 0:837acb06c892 63 selector_a = 1;
rizqicahyo 0:837acb06c892 64 selector_b = 1;
rizqicahyo 0:837acb06c892 65 selector_c = 1;
rizqicahyo 0:837acb06c892 66 }
rizqicahyo 0:837acb06c892 67 }
rizqicahyo 0:837acb06c892 68 char sensor_ki (int urutan)
rizqicahyo 0:837acb06c892 69 {
rizqicahyo 0:837acb06c892 70 char x;
rizqicahyo 0:837acb06c892 71 //int hasil;
rizqicahyo 0:837acb06c892 72 Selektor(urutan);
rizqicahyo 0:837acb06c892 73 wait_ms(1);
rizqicahyo 0:837acb06c892 74 if (sensorkiri.read() >= 1)
rizqicahyo 0:837acb06c892 75 {
rizqicahyo 0:837acb06c892 76
rizqicahyo 0:837acb06c892 77 x='1';
rizqicahyo 0:837acb06c892 78 }
rizqicahyo 0:837acb06c892 79 else
rizqicahyo 0:837acb06c892 80 {
rizqicahyo 0:837acb06c892 81 x='0';
rizqicahyo 0:837acb06c892 82 }
rizqicahyo 0:837acb06c892 83
rizqicahyo 0:837acb06c892 84 return x;
rizqicahyo 0:837acb06c892 85 }
rizqicahyo 0:837acb06c892 86 char sensor_ka (int urutan )
rizqicahyo 0:837acb06c892 87 {
rizqicahyo 0:837acb06c892 88 char x;
rizqicahyo 0:837acb06c892 89 //int hasil ;
rizqicahyo 0:837acb06c892 90 Selektor(urutan);
rizqicahyo 0:837acb06c892 91 wait_ms(1);
rizqicahyo 0:837acb06c892 92 if (sensorkanan.read() >= 1)
rizqicahyo 0:837acb06c892 93 {
rizqicahyo 0:837acb06c892 94 x='1';
rizqicahyo 0:837acb06c892 95 }
rizqicahyo 0:837acb06c892 96 else
rizqicahyo 0:837acb06c892 97 {
rizqicahyo 0:837acb06c892 98 x='0';
rizqicahyo 0:837acb06c892 99 }
rizqicahyo 0:837acb06c892 100
rizqicahyo 0:837acb06c892 101 return x;
rizqicahyo 0:837acb06c892 102 }
rizqicahyo 0:837acb06c892 103
rizqicahyo 0:837acb06c892 104 float sensor_ki_adc (int urutan)
rizqicahyo 0:837acb06c892 105 {
rizqicahyo 0:837acb06c892 106 //int hasil;
rizqicahyo 0:837acb06c892 107 Selektor(urutan);
rizqicahyo 0:837acb06c892 108 wait_ms(1);
rizqicahyo 0:837acb06c892 109
rizqicahyo 0:837acb06c892 110 return sensorkiri.read();
rizqicahyo 0:837acb06c892 111 }
rizqicahyo 0:837acb06c892 112 float sensor_ka_adc (int urutan )
rizqicahyo 0:837acb06c892 113 {
rizqicahyo 0:837acb06c892 114 //int hasil ;
rizqicahyo 0:837acb06c892 115 Selektor(urutan);
rizqicahyo 0:837acb06c892 116 wait_ms(1);
rizqicahyo 0:837acb06c892 117
rizqicahyo 0:837acb06c892 118 return sensorkanan.read();
rizqicahyo 0:837acb06c892 119 }
rizqicahyo 0:837acb06c892 120
rizqicahyo 0:837acb06c892 121 void tampil_lcd_bin()
rizqicahyo 0:837acb06c892 122 {
rizqicahyo 0:837acb06c892 123 lcd.locate(2,3);
rizqicahyo 0:837acb06c892 124 lcd.putc(sensor_ki(7));
rizqicahyo 0:837acb06c892 125 lcd.locate(3,3);
rizqicahyo 0:837acb06c892 126 //pc.printf (" ");
rizqicahyo 0:837acb06c892 127 lcd.putc(sensor_ki(6));
rizqicahyo 0:837acb06c892 128 lcd.locate(4,3);
rizqicahyo 0:837acb06c892 129 //pc.printf (" ");
rizqicahyo 0:837acb06c892 130 lcd.putc(sensor_ki(5));
rizqicahyo 0:837acb06c892 131 lcd.locate(5,3);
rizqicahyo 0:837acb06c892 132 lcd.putc(sensor_ki(4));
rizqicahyo 0:837acb06c892 133 lcd.locate(6,3);
rizqicahyo 0:837acb06c892 134 lcd.putc(sensor_ki(3));
rizqicahyo 0:837acb06c892 135 lcd.locate(7,3);
rizqicahyo 0:837acb06c892 136 lcd.putc(sensor_ki(2));
rizqicahyo 0:837acb06c892 137 lcd.locate(8,3);
rizqicahyo 0:837acb06c892 138 lcd.putc(sensor_ki(1));
rizqicahyo 0:837acb06c892 139 lcd.locate(9,3);
rizqicahyo 0:837acb06c892 140 lcd.putc(sensor_ki(0));
rizqicahyo 0:837acb06c892 141 lcd.locate(10,3);
rizqicahyo 0:837acb06c892 142 lcd.putc(sensor_ka(7));
rizqicahyo 0:837acb06c892 143 lcd.locate(11,3);
rizqicahyo 0:837acb06c892 144 lcd.putc(sensor_ka(6));
rizqicahyo 0:837acb06c892 145 lcd.locate(12,3);
rizqicahyo 0:837acb06c892 146 lcd.putc(sensor_ka(5));
rizqicahyo 0:837acb06c892 147 lcd.locate(13,3);
rizqicahyo 0:837acb06c892 148 lcd.putc(sensor_ka(4));
rizqicahyo 0:837acb06c892 149 lcd.locate(14,3);
rizqicahyo 0:837acb06c892 150 lcd.putc(sensor_ka(3));
rizqicahyo 0:837acb06c892 151 lcd.locate(15,3);
rizqicahyo 0:837acb06c892 152 lcd.putc(sensor_ka(2));
rizqicahyo 0:837acb06c892 153 lcd.locate(16,3);
rizqicahyo 0:837acb06c892 154 lcd.putc(sensor_ka(1));
rizqicahyo 0:837acb06c892 155 lcd.locate(17,3);
rizqicahyo 0:837acb06c892 156 lcd.putc(sensor_ka(0));
rizqicahyo 0:837acb06c892 157 lcd.locate(18,3);
rizqicahyo 0:837acb06c892 158 }
rizqicahyo 0:837acb06c892 159
rizqicahyo 0:837acb06c892 160 void tampil_lcd_adc()
rizqicahyo 0:837acb06c892 161 {
rizqicahyo 0:837acb06c892 162 int k,j,i;
rizqicahyo 0:837acb06c892 163 i=0;
rizqicahyo 0:837acb06c892 164
rizqicahyo 0:837acb06c892 165 while(i <= 3)
rizqicahyo 0:837acb06c892 166 {
rizqicahyo 0:837acb06c892 167 j=0;
rizqicahyo 0:837acb06c892 168 k=7;
rizqicahyo 0:837acb06c892 169 while(j<16 && k>3)
rizqicahyo 0:837acb06c892 170 {
rizqicahyo 0:837acb06c892 171 lcd.locate(j,i);
rizqicahyo 0:837acb06c892 172 if(i==0)
rizqicahyo 0:837acb06c892 173 {
rizqicahyo 0:837acb06c892 174 //lcd.locate(j,i);
rizqicahyo 0:837acb06c892 175 lcd.printf("%1.2f",sensor_ki_adc(k));
rizqicahyo 0:837acb06c892 176 }
rizqicahyo 0:837acb06c892 177 else if(i==1)
rizqicahyo 0:837acb06c892 178 {
rizqicahyo 0:837acb06c892 179 //lcd.locate(j,i);
rizqicahyo 0:837acb06c892 180 lcd.printf("%1.2f",sensor_ki_adc(k-4));
rizqicahyo 0:837acb06c892 181 }
rizqicahyo 0:837acb06c892 182 else if(i==2)
rizqicahyo 0:837acb06c892 183 {
rizqicahyo 0:837acb06c892 184 //lcd.locate(j,i);
rizqicahyo 0:837acb06c892 185 lcd.printf("%1.2f",sensor_ka_adc(k));
rizqicahyo 0:837acb06c892 186 }
rizqicahyo 0:837acb06c892 187 else if(i==3)
rizqicahyo 0:837acb06c892 188 {
rizqicahyo 0:837acb06c892 189 //lcd.locate(j,i);
rizqicahyo 0:837acb06c892 190 lcd.printf("%1.2f",sensor_ka_adc(k-4));
rizqicahyo 0:837acb06c892 191 }
rizqicahyo 0:837acb06c892 192
rizqicahyo 0:837acb06c892 193 j=j+5;
rizqicahyo 0:837acb06c892 194 k--;
rizqicahyo 0:837acb06c892 195 }
rizqicahyo 0:837acb06c892 196 i++;
rizqicahyo 0:837acb06c892 197 }
rizqicahyo 0:837acb06c892 198 }
rizqicahyo 0:837acb06c892 199
rizqicahyo 0:837acb06c892 200
rizqicahyo 0:837acb06c892 201
rizqicahyo 0:837acb06c892 202 int main()
rizqicahyo 0:837acb06c892 203 {
rizqicahyo 0:837acb06c892 204 pc.baud(115200);
rizqicahyo 0:837acb06c892 205 //pc.printf("Pembacaan ADC Sensor : \n");
rizqicahyo 0:837acb06c892 206 while (1)
rizqicahyo 0:837acb06c892 207 {
rizqicahyo 0:837acb06c892 208 if(but0 == 1)
rizqicahyo 0:837acb06c892 209 {
rizqicahyo 0:837acb06c892 210 tampil_lcd_adc();
rizqicahyo 0:837acb06c892 211 //wait_ms(100);
rizqicahyo 0:837acb06c892 212 }
rizqicahyo 0:837acb06c892 213 else
rizqicahyo 0:837acb06c892 214 {
rizqicahyo 0:837acb06c892 215 lcd.locate(0,0);
rizqicahyo 0:837acb06c892 216 lcd.printf("rizqi cahyo Yuwono");
rizqicahyo 0:837acb06c892 217 lcd.locate(0,1);
rizqicahyo 0:837acb06c892 218 lcd.printf("12314090");
rizqicahyo 0:837acb06c892 219 tampil_lcd_bin();
rizqicahyo 0:837acb06c892 220 }
rizqicahyo 0:837acb06c892 221
rizqicahyo 0:837acb06c892 222 pc.printf ("%f", sensor_ki_adc(7));
rizqicahyo 0:837acb06c892 223 //pc.printf (" ");
rizqicahyo 0:837acb06c892 224 pc.printf (" %f ", sensor_ki_adc(6));
rizqicahyo 0:837acb06c892 225 //pc.printf (" ");
rizqicahyo 0:837acb06c892 226 pc.printf (" %f ", sensor_ki_adc(5));
rizqicahyo 0:837acb06c892 227 pc.printf (" %f ", sensor_ki_adc(4));
rizqicahyo 0:837acb06c892 228 pc.printf (" %f ", sensor_ki_adc(3));
rizqicahyo 0:837acb06c892 229 pc.printf (" %f ", sensor_ki_adc(2));
rizqicahyo 0:837acb06c892 230 pc.printf (" %f ", sensor_ki_adc(1));
rizqicahyo 0:837acb06c892 231 pc.printf (" %f ", sensor_ki_adc(0));
rizqicahyo 0:837acb06c892 232 pc.printf ("\t\t");
rizqicahyo 0:837acb06c892 233 pc.printf (" %f ", sensor_ka_adc(7));
rizqicahyo 0:837acb06c892 234 pc.printf (" %f ", sensor_ka_adc(6));
rizqicahyo 0:837acb06c892 235 pc.printf (" %f ", sensor_ka_adc(5));
rizqicahyo 0:837acb06c892 236 pc.printf (" %f ", sensor_ka_adc(4));
rizqicahyo 0:837acb06c892 237 pc.printf (" %f ", sensor_ka_adc(3));
rizqicahyo 0:837acb06c892 238 pc.printf (" %f ", sensor_ka_adc(2));
rizqicahyo 0:837acb06c892 239 pc.printf (" %f ", sensor_ka_adc(1));
rizqicahyo 0:837acb06c892 240 pc.printf (" %f ", sensor_ka_adc(0));
rizqicahyo 0:837acb06c892 241 pc.printf ("\r\n");
rizqicahyo 0:837acb06c892 242
rizqicahyo 0:837acb06c892 243 wait_ms(10);
rizqicahyo 0:837acb06c892 244 lcd.cls();
rizqicahyo 0:837acb06c892 245 }
rizqicahyo 0:837acb06c892 246 }
rizqicahyo 0:837acb06c892 247