tes ir atas semua
Dependencies: mbed ADS1115 StepperMotor SRF05 TPA81new
main.cpp@49:d23d76689933, 2019-03-06 (annotated)
- Committer:
- hisyamfs
- Date:
- Wed Mar 06 13:17:10 2019 +0000
- Revision:
- 49:d23d76689933
- Parent:
- 48:c8c9c624d46d
tes bisa semua, tanpa servo. uvtron nggak bisa
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
hisyamfs | 12:1e3227a6fcd7 | 1 | #include "mbed.h" |
hisyamfs | 12:1e3227a6fcd7 | 2 | #include "TPA81new.h" |
hisyamfs | 16:771d84a80e3d | 3 | #include "SRF05.h" |
hisyamfs | 19:9a7b34bedc6c | 4 | #include "Uvtron.h" |
hisyamfs | 41:71c6f608fd2b | 5 | #include "Stepper.h" |
hisyamfs | 42:3b682b7d0388 | 6 | #include "AMG8833.h" |
hisyamfs | 44:49ef8addf233 | 7 | #include "Adafruit_ADS1015.h" |
hisyamfs | 44:49ef8addf233 | 8 | |
hisyamfs | 44:49ef8addf233 | 9 | // Konstanta |
hisyamfs | 49:d23d76689933 | 10 | #define WAIT_TIME 0.02 |
hisyamfs | 44:49ef8addf233 | 11 | // infrared |
hisyamfs | 44:49ef8addf233 | 12 | #define a_depan 5000000 |
hisyamfs | 44:49ef8addf233 | 13 | #define b_depan 1.229 |
hisyamfs | 44:49ef8addf233 | 14 | #define a_ads 500000 |
hisyamfs | 44:49ef8addf233 | 15 | #define b_ads 1.184 |
hisyamfs | 49:d23d76689933 | 16 | #define I2C_ADDR 0xC0 |
hisyamfs | 14:207770fefedf | 17 | |
hisyamfs | 49:d23d76689933 | 18 | Serial pc(USBTX,USBRX, 115200); |
hisyamfs | 17:fd06e8104835 | 19 | |
hisyamfs | 42:3b682b7d0388 | 20 | // AMG |
hisyamfs | 44:49ef8addf233 | 21 | Adafruit_AMG88xx amg(PB_3, PB_10); // sda, scl |
hisyamfs | 42:3b682b7d0388 | 22 | float pixels[AMG88xx_PIXEL_ARRAY_SIZE]; |
dmgongora | 0:79e2a8171b16 | 23 | |
hisyamfs | 18:f3b23331090f | 24 | DigitalOut relay(PB_2); |
hisyamfs | 44:49ef8addf233 | 25 | Uvtron uv(PC_12); |
hisyamfs | 26:9c7f39e21df4 | 26 | DigitalIn sound(PA_9); |
hisyamfs | 21:bfb064f77f88 | 27 | AnalogIn line(PC_1); |
hisyamfs | 21:bfb064f77f88 | 28 | |
hisyamfs | 45:96815f504b17 | 29 | // Infrared |
hisyamfs | 27:4d79aba70fe8 | 30 | // atas |
hisyamfs | 27:4d79aba70fe8 | 31 | AnalogIn ira1(PC_5); |
hisyamfs | 27:4d79aba70fe8 | 32 | AnalogIn ira2(PA_5); |
hisyamfs | 27:4d79aba70fe8 | 33 | AnalogIn ira3(PC_2); |
hisyamfs | 27:4d79aba70fe8 | 34 | AnalogIn ira4(PC_3); |
hisyamfs | 44:49ef8addf233 | 35 | AnalogIn ira5(PC_4); |
hisyamfs | 44:49ef8addf233 | 36 | AnalogIn ira6(PB_1); |
hisyamfs | 44:49ef8addf233 | 37 | AnalogIn ira7(PA_7); |
hisyamfs | 44:49ef8addf233 | 38 | AnalogIn ira8(PA_6); |
hisyamfs | 27:4d79aba70fe8 | 39 | // bawah |
hisyamfs | 27:4d79aba70fe8 | 40 | AnalogIn irb1(PA_0); |
hisyamfs | 27:4d79aba70fe8 | 41 | AnalogIn irb2(PA_1); |
hisyamfs | 27:4d79aba70fe8 | 42 | AnalogIn irb3(PA_4); |
hisyamfs | 27:4d79aba70fe8 | 43 | AnalogIn irb4(PB_0); |
hisyamfs | 44:49ef8addf233 | 44 | // ADC |
hisyamfs | 44:49ef8addf233 | 45 | I2C ads_i2c(PB_9, PB_8); // sda, scl |
hisyamfs | 45:96815f504b17 | 46 | Adafruit_ADS1015 ads(&ads_i2c); |
hisyamfs | 27:4d79aba70fe8 | 47 | |
hisyamfs | 49:d23d76689933 | 48 | // kompas |
hisyamfs | 49:d23d76689933 | 49 | I2C cmps12(PB_4, PA_8); //sda, scl |
hisyamfs | 49:d23d76689933 | 50 | |
hisyamfs | 49:d23d76689933 | 51 | // stepper(PinName _en, PinName ms1, PinName ms2, PinName ms3, PinName _stepPin, PinName dir); |
hisyamfs | 45:96815f504b17 | 52 | stepper s(PC_8, PC_6, PA_12, PA_11, PB_12, PC_7); |
hisyamfs | 49:d23d76689933 | 53 | |
hisyamfs | 49:d23d76689933 | 54 | // sound |
hisyamfs | 49:d23d76689933 | 55 | //DigitalIn sound(PA_9); |
hisyamfs | 40:1c29f33d54d7 | 56 | |
hisyamfs | 44:49ef8addf233 | 57 | DigitalOut led_api(PB_6); |
hisyamfs | 37:f0b535131cbf | 58 | DigitalOut led_sound(PH_1); |
hisyamfs | 37:f0b535131cbf | 59 | |
hisyamfs | 45:96815f504b17 | 60 | float call_depan(uint16_t x) |
hisyamfs | 45:96815f504b17 | 61 | { |
hisyamfs | 27:4d79aba70fe8 | 62 | float side_distance = a_depan/pow(x,b_depan)+1; |
hisyamfs | 27:4d79aba70fe8 | 63 | return side_distance; |
hisyamfs | 27:4d79aba70fe8 | 64 | } |
hisyamfs | 27:4d79aba70fe8 | 65 | |
hisyamfs | 44:49ef8addf233 | 66 | float call_ads(uint16_t ir) { |
hisyamfs | 44:49ef8addf233 | 67 | float voltage = ir * 6.2676 * 10e-5; // ubah ke mV |
hisyamfs | 44:49ef8addf233 | 68 | uint16_t x = (voltage/3.3f) * 0xFFFF; // ubah ke uint16_t sesuai max nucleo |
hisyamfs | 45:96815f504b17 | 69 | float dist = a_ads/pow(x, b_ads)+1; |
hisyamfs | 44:49ef8addf233 | 70 | return dist; |
hisyamfs | 44:49ef8addf233 | 71 | } |
hisyamfs | 44:49ef8addf233 | 72 | |
dmgongora | 0:79e2a8171b16 | 73 | int main() |
hisyamfs | 12:1e3227a6fcd7 | 74 | { |
hisyamfs | 46:4030083b43b3 | 75 | uint16_t ads_raw[4] = {}; |
hisyamfs | 49:d23d76689933 | 76 | int status = amg.begin(AMG88xx_ADDRESS); |
hisyamfs | 49:d23d76689933 | 77 | amg.setMovingAverageMode(1); |
hisyamfs | 49:d23d76689933 | 78 | if (!status) { |
hisyamfs | 49:d23d76689933 | 79 | pc.printf("Could not find a valid AMG88xx sensor, check wiring! \n"); |
hisyamfs | 49:d23d76689933 | 80 | while (1); |
hisyamfs | 49:d23d76689933 | 81 | } |
hisyamfs | 49:d23d76689933 | 82 | relay = 1; |
hisyamfs | 49:d23d76689933 | 83 | wait_ms(100); |
hisyamfs | 49:d23d76689933 | 84 | while (sound.read() == 1){ |
hisyamfs | 49:d23d76689933 | 85 | wait_ms(100); |
hisyamfs | 49:d23d76689933 | 86 | }; |
hisyamfs | 49:d23d76689933 | 87 | pc.printf("Sound detected\n"); |
hisyamfs | 45:96815f504b17 | 88 | while (1) { |
hisyamfs | 49:d23d76689933 | 89 | led_sound = 1; |
hisyamfs | 42:3b682b7d0388 | 90 | // IR |
hisyamfs | 48:c8c9c624d46d | 91 | for (uint8_t i=0; i < 4; i++) { |
hisyamfs | 48:c8c9c624d46d | 92 | ads_raw[i] = ads.readADC_SingleEnded(i); |
hisyamfs | 48:c8c9c624d46d | 93 | wait_ms(10); |
hisyamfs | 48:c8c9c624d46d | 94 | }; |
hisyamfs | 27:4d79aba70fe8 | 95 | uint16_t raw_a1 = ira1.read_u16(); |
hisyamfs | 27:4d79aba70fe8 | 96 | uint16_t raw_a2 = ira2.read_u16(); |
hisyamfs | 27:4d79aba70fe8 | 97 | uint16_t raw_a3 = ira3.read_u16(); |
hisyamfs | 27:4d79aba70fe8 | 98 | uint16_t raw_a4 = ira4.read_u16(); |
hisyamfs | 27:4d79aba70fe8 | 99 | uint16_t raw_a5 = ira5.read_u16(); |
hisyamfs | 27:4d79aba70fe8 | 100 | uint16_t raw_a6 = ira6.read_u16(); |
hisyamfs | 27:4d79aba70fe8 | 101 | uint16_t raw_a7 = ira7.read_u16(); |
hisyamfs | 27:4d79aba70fe8 | 102 | uint16_t raw_a8 = ira8.read_u16(); |
hisyamfs | 45:96815f504b17 | 103 | |
hisyamfs | 27:4d79aba70fe8 | 104 | uint16_t raw_b1 = irb1.read_u16(); |
hisyamfs | 27:4d79aba70fe8 | 105 | uint16_t raw_b2 = irb2.read_u16(); |
hisyamfs | 27:4d79aba70fe8 | 106 | uint16_t raw_b3 = irb3.read_u16(); |
hisyamfs | 27:4d79aba70fe8 | 107 | uint16_t raw_b4 = irb4.read_u16(); |
hisyamfs | 45:96815f504b17 | 108 | |
hisyamfs | 27:4d79aba70fe8 | 109 | float dist_a1 = call_depan(raw_a1); |
hisyamfs | 27:4d79aba70fe8 | 110 | float dist_a2 = call_depan(raw_a2); |
hisyamfs | 27:4d79aba70fe8 | 111 | float dist_a3 = call_depan(raw_a3); |
hisyamfs | 27:4d79aba70fe8 | 112 | float dist_a4 = call_depan(raw_a4); |
hisyamfs | 27:4d79aba70fe8 | 113 | float dist_a5 = call_depan(raw_a5); |
hisyamfs | 27:4d79aba70fe8 | 114 | float dist_a6 = call_depan(raw_a6); |
hisyamfs | 27:4d79aba70fe8 | 115 | float dist_a7 = call_depan(raw_a7); |
hisyamfs | 27:4d79aba70fe8 | 116 | float dist_a8 = call_depan(raw_a8); |
hisyamfs | 48:c8c9c624d46d | 117 | float dist_a9 = call_ads(ads_raw[1]); |
hisyamfs | 48:c8c9c624d46d | 118 | float dist_a10 = call_ads(ads_raw[3]); |
hisyamfs | 48:c8c9c624d46d | 119 | float dist_a11 = call_ads(ads_raw[2]); |
hisyamfs | 48:c8c9c624d46d | 120 | float dist_a12 = call_ads(ads_raw[0]); |
hisyamfs | 27:4d79aba70fe8 | 121 | float dist_b1 = call_depan(raw_b1); |
hisyamfs | 27:4d79aba70fe8 | 122 | float dist_b2 = call_depan(raw_b2); |
hisyamfs | 27:4d79aba70fe8 | 123 | float dist_b3 = call_depan(raw_b3); |
hisyamfs | 27:4d79aba70fe8 | 124 | float dist_b4 = call_depan(raw_b4); |
hisyamfs | 45:96815f504b17 | 125 | |
hisyamfs | 27:4d79aba70fe8 | 126 | pc.printf("ir atas\n"); |
hisyamfs | 48:c8c9c624d46d | 127 | pc.printf("%.2f %.2f %.2f %.2f\n", dist_a9, dist_a10, dist_a11, dist_a12); |
hisyamfs | 44:49ef8addf233 | 128 | pc.printf("%.2f %.2f\n", dist_a3, dist_a1); |
hisyamfs | 44:49ef8addf233 | 129 | pc.printf("%.2f %.2f\n", dist_a4, dist_a6); |
hisyamfs | 44:49ef8addf233 | 130 | pc.printf("%.2f %.2f %.2f %.2f\n", dist_a2, dist_a8, dist_a7, dist_a5); |
hisyamfs | 45:96815f504b17 | 131 | |
hisyamfs | 27:4d79aba70fe8 | 132 | pc.printf("ir bawah\n"); |
hisyamfs | 28:528c2fbd6a54 | 133 | pc.printf(" %.2f\n", dist_b1); |
hisyamfs | 28:528c2fbd6a54 | 134 | pc.printf("%.2f %.2f\n", dist_b2, dist_b3); |
hisyamfs | 28:528c2fbd6a54 | 135 | pc.printf(" %.2f\n", dist_b4); |
hisyamfs | 49:d23d76689933 | 136 | |
hisyamfs | 49:d23d76689933 | 137 | // baca amg |
hisyamfs | 49:d23d76689933 | 138 | float therm_temp = amg.readThermistor(); |
hisyamfs | 49:d23d76689933 | 139 | amg.readPixels(pixels); |
hisyamfs | 49:d23d76689933 | 140 | pc.printf("%.2f\n", therm_temp); |
hisyamfs | 49:d23d76689933 | 141 | float max_temp = pixels[0]; |
hisyamfs | 49:d23d76689933 | 142 | int max_i = 0; |
hisyamfs | 49:d23d76689933 | 143 | float sum_temp = 0; |
hisyamfs | 49:d23d76689933 | 144 | for (int i = 1; i <= AMG88xx_PIXEL_ARRAY_SIZE; i++) { |
hisyamfs | 49:d23d76689933 | 145 | // pc.printf("%.2f ", pixels[i-1]); |
hisyamfs | 49:d23d76689933 | 146 | // if (i % 8 == 0) pc.printf("\n"); |
hisyamfs | 49:d23d76689933 | 147 | if (pixels[i-1] > max_temp) { |
hisyamfs | 49:d23d76689933 | 148 | max_temp = pixels[i-1]; |
hisyamfs | 49:d23d76689933 | 149 | max_i = i; |
hisyamfs | 49:d23d76689933 | 150 | } |
hisyamfs | 49:d23d76689933 | 151 | sum_temp += pixels[i-1]; |
hisyamfs | 49:d23d76689933 | 152 | } |
hisyamfs | 49:d23d76689933 | 153 | |
hisyamfs | 49:d23d76689933 | 154 | // uvtron |
hisyamfs | 49:d23d76689933 | 155 | uv.Read(); |
hisyamfs | 49:d23d76689933 | 156 | int read = uv.Flag; |
hisyamfs | 49:d23d76689933 | 157 | if (read) pc.printf("FIRE DETECTED\n"); |
hisyamfs | 49:d23d76689933 | 158 | else pc.printf("NOT DETECTED\n"); |
hisyamfs | 49:d23d76689933 | 159 | |
hisyamfs | 49:d23d76689933 | 160 | pc.printf("temp max= %.2f at %d\n", max_temp, max_i); |
hisyamfs | 49:d23d76689933 | 161 | pc.printf("temp rata2 = %.2f\n", sum_temp/AMG88xx_PIXEL_ARRAY_SIZE); |
hisyamfs | 49:d23d76689933 | 162 | |
hisyamfs | 49:d23d76689933 | 163 | // baca kompas |
hisyamfs | 49:d23d76689933 | 164 | char data[2] = {0x1, 0xFF}; |
hisyamfs | 49:d23d76689933 | 165 | int nack; |
hisyamfs | 49:d23d76689933 | 166 | nack = cmps12.write(I2C_ADDR, data, 1); |
hisyamfs | 49:d23d76689933 | 167 | nack = cmps12.read(I2C_ADDR, data+1, 1); |
hisyamfs | 49:d23d76689933 | 168 | pc.printf("Bearing kompas = %.2f\n", (float) data[1]/255*360); |
hisyamfs | 49:d23d76689933 | 169 | |
hisyamfs | 49:d23d76689933 | 170 | // stepper |
hisyamfs | 49:d23d76689933 | 171 | pc.printf("Stepper \n"); |
hisyamfs | 49:d23d76689933 | 172 | for (int i = 0; i < 50; i++) { |
hisyamfs | 49:d23d76689933 | 173 | s.step(1, 1, 1/WAIT_TIME); // CCW |
hisyamfs | 49:d23d76689933 | 174 | } |
hisyamfs | 49:d23d76689933 | 175 | |
hisyamfs | 49:d23d76689933 | 176 | for (int i = 0; i < 50; i++) { |
hisyamfs | 49:d23d76689933 | 177 | s.step(1, 0, 1/WAIT_TIME); // CW |
hisyamfs | 49:d23d76689933 | 178 | } |
hisyamfs | 49:d23d76689933 | 179 | |
hisyamfs | 49:d23d76689933 | 180 | // pompa |
hisyamfs | 49:d23d76689933 | 181 | relay = 0; // active low |
hisyamfs | 49:d23d76689933 | 182 | pc.printf("Pompa on!\n"); |
hisyamfs | 49:d23d76689933 | 183 | wait(1); |
hisyamfs | 49:d23d76689933 | 184 | relay = 1; |
hisyamfs | 49:d23d76689933 | 185 | pc.printf("Pompa off\n"); |
hisyamfs | 49:d23d76689933 | 186 | wait(1); |
hisyamfs | 49:d23d76689933 | 187 | |
hisyamfs | 45:96815f504b17 | 188 | wait_ms(200); |
hisyamfs | 49:d23d76689933 | 189 | |
hisyamfs | 37:f0b535131cbf | 190 | pc.printf("\033[2J\033[H"); |
hisyamfs | 14:207770fefedf | 191 | } |
hisyamfs | 15:da7a15289893 | 192 | } |