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@1:2bf3dac65b08, 2015-11-24 (annotated)

Committer:
rizqicahyo
Date:
Tue Nov 24 14:39:59 2015 +0000
Revision:
1:2bf3dac65b08
Parent:
0:837acb06c892
Child:
2:907766ac29a2
tugas hybrid LCD

Who changed what in which revision?

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