Diff: main.cpp

Revision:

0:ac8a5576c57a

Child:

1:c5bfd182c743

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Aug 23 16:09:04 2019 +0000
@@ -0,0 +1,225 @@
+ #include "mbed.h"
+#include <string> 
+#include <math.h>
+//#include "SSD1308.h"
+
+#define AVG 5
+#define cal_num 10
+
+AnalogIn a1(PA_0);
+AnalogIn a2(PA_1);
+AnalogIn a3(PA_3);
+AnalogIn a4(PA_4);
+AnalogIn a5(PA_5);
+AnalogIn a6(PA_6);
+AnalogIn abat(PA_7);
+
+DigitalIn SW1(PA_11);
+DigitalIn SW2(PB_5);
+DigitalIn SW3(PB_4);
+DigitalIn JSW1(PA_12);
+DigitalIn JSW2(PB_0);
+DigitalIn JSW3(PA_8);
+DigitalIn TSW(PB_1);
+Serial hc(PA_9, PA_10);
+Serial pc(PA_2, PA_15);
+//Serial pc(SERIAL_TX, SERIAL_RX);
+I2C i2c(PB_7, PB_6);
+//SSD1308 oled = SSD1308(&i2c, SSD1308_SA0);
+
+char data[300];
+int i=0;
+char ndone=1;
+char avg_count=0;
+
+float LAT,LON,TIME;
+char NS,EW,status;
+float check;
+int latdeg,londeg;
+float latmin,lonmin;
+float lat_arr[AVG];
+float lon_arr[AVG];
+
+//short x1,x2,x3,y1,y2,y3;
+short TH,PI,RO,YA,TH_def;//,AUX,LAT,LON,COM
+float x1_ar,x2_ar,x3_ar,y1_ar,y2_ar,y3_ar;
+short mid_point=50;
+short x1_cor,x2_cor,x3_cor,y1_cor,y2_cor,y3_cor;
+short low_us=1350;
+short multiplier_us=3;
+int rcom=0;
+int arcom;
+
+void reads();
+void rounder();
+int get_deg(float);
+float get_min(float);
+
+float vbat;
+float bat_divider=0.703313;
+
+char oled_str[30];
+
+void avg_read();
+void calibration();
+void th_calc();
+
+int main(){
+    SW1.mode(PullDown);
+    SW2.mode(PullDown);
+    SW3.mode(PullDown);
+    JSW1.mode(PullDown);
+    JSW2.mode(PullUp);
+    JSW3.mode(PullUp);//default 1 uvjet ako 0
+    TSW.mode(PullDown);
+    calibration();
+    //oled.setDisplayFlip(false,false);
+    //oled.setContrastControl(0xFF);
+    while(1){
+        //oled.clearDisplay();
+        sprintf(oled_str, "%d", TH_def);
+        //oled.writeString(0, 0, oled_str);
+        if(TSW.read()){
+            //reads();
+            //avg_read();
+            //rounder();
+            //nazvat TH,
+            TH =(-1)*((((a1.read()*100)+x1_cor)*5)-250);
+            //pc.printf("%hd",TH);
+            th_calc();
+            RO =3000-(low_us+((a3.read()*100)+y1_cor)*multiplier_us); 
+            YA =3000-(low_us+((a2.read()*100)+x2_cor)*multiplier_us);
+            PI =3000-(low_us+((a4.read()*100)+y2_cor)*multiplier_us);
+            arcom=a5.read()*100;
+            rcom=0;
+            if(SW2.read()==1){
+                rcom=2;
+            }
+            if(SW3.read()==1){
+                hc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",1000,1000,1000,1000,2000,0,0,0);
+            }
+            else{
+                hc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",TH_def,PI,RO,YA,1000,0,0,rcom);
+            }
+            //na drugom kraju cekaj MSG
+            //printf("%hd,%hd,%hd,%hd,%hd,%hd\n",x1,y1,x2,y2,sw1.read(),sw2.read());
+            //printf("%hd,%hd,%hd,%hd\n",sw1.read(),sw2.read(),sw3.read(),tog1.read());
+            //vbat=(abat.read()*3.3f)/bat_divider;
+            
+            //printf("%0.2f\n",vbat);
+            wait_ms(10);// izmjerit potrebno vrijeme da se izvrsi sve to pa stavit na temelju toga
+        
+        }
+        else{
+            //mjenjanje argumentata ekran itd itd
+            //send disarm
+            //hc_send()
+            
+            hc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",1000,1500,1500,1500,2000,0,0,0);
+            wait_ms(50);
+        
+        }
+    }
+}
+/*
+
+void reads(){
+    while(avg_count<AVG){
+        while(gps.getc() != '$');
+        while(ndone){
+            data[i]=gps.getc();
+            pc.printf("%c",data[i]);
+            if(data[i]=='\r'){
+                data[i]='\0';
+                ndone=0;
+            }
+            i++;
+        }
+        //GPGLL,4540.45939,N,01539.27211,E,164202.00,A,A*64
+        if(sscanf(data,"GPGLL,%f,%c,%f,%c,%f,%c,%f",&LAT, &NS, &LON, &EW, &TIME, &status, &check) >= 1) {
+            pc.printf("%s\n",data);
+            lat_arr[avg_count]=LAT;
+            lon_arr[avg_count]=LON;
+            avg_count++;
+            
+                    time = time + 20000.00f;
+                    hour = int(time) / 10000;
+                    minute = (int(time) % 10000) / 100;
+                    seconed = int(time) % 100;
+             
+             
+        }
+        memset(data, 0, i);
+        ndone=1;
+        i=0;
+    }
+}
+
+*/
+void rounder(){ 
+     latdeg=get_deg(LAT);
+     londeg=get_deg(LON);
+     latmin=get_min(LAT);
+     lonmin=get_min(LON);
+     pc.printf("%d %.3f, %d %.3f\n",latdeg,latmin,londeg,lonmin);
+}
+
+int get_deg(float fnum){
+    return floor(fnum/100.0f);
+}
+
+float get_min(float fnum){
+    unsigned int tempmini=(int(fnum*1000))%100000;
+    return tempmini/1000.0f;
+}
+
+void avg_read(){
+    LAT=0;
+    LON=0;
+    for(int k=0;k<AVG;k++){
+        LAT+=lat_arr[k];
+        LON+=lon_arr[k];
+    }
+    LAT=LAT/AVG;
+    LON=LON/AVG;
+}
+
+void calibration(){
+    x1_ar=0;
+    x2_ar=0;
+    x3_ar=0;
+    y1_ar=0;
+    y2_ar=0;
+    y3_ar=0;
+    for(int a=0;a<cal_num;a++){
+        x1_ar+=a1.read();
+        x2_ar+=a2.read();
+        x3_ar+=a5.read(); 
+        y1_ar+=a3.read();
+        y2_ar+=a4.read();
+        y3_ar+=a6.read();       
+    }
+    x1_cor=mid_point-((x1_ar/cal_num)*100);
+    x2_cor=mid_point-((x2_ar/cal_num)*100);
+    x3_cor=mid_point-((x3_ar/cal_num)*100);
+    y1_cor=mid_point-((y1_ar/cal_num)*100);
+    y2_cor=mid_point-((y2_ar/cal_num)*100);
+    y3_cor=mid_point-((y3_ar/cal_num)*100);
+    pc.printf("%hd,%hd,%hd,%hd,%hd,%hd\n",x1_cor,y1_cor,x2_cor,y2_cor,x3_cor,y3_cor);
+}
+
+void th_calc(){
+    if(abs(TH)<10){
+        TH_def+=0;
+    }
+    else{
+        if(TH>0){
+            TH_def+=10;
+        }
+        else{
+            TH_def-=10;
+        }
+    }
+    if(TH_def<1000)TH_def=1000;
+    if(JSW1.read()==1)TH_def=1000;
+}
\ No newline at end of file