File content as of revision 0:ac8a5576c57a:

 #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;
}