File content as of revision 1:4c27d179c6c2:

#include "mbed.h"
//#include <math.h> //Probo sma bez toga radi sve pa nije ni potrebno mozda tj tamo kad ubacis onaj library i kod za akcelometar ce trebat mozda vidi sta taj main ima includano
//#include "SSD1308.h" //lib za ekran to cu ti isto donjet jedan dan

//#define AVG 5 //Broj mjerenja za pocetni gps, ne koristi se jer nema gpsa
#define cal_num 10 // broj ocitanja analognih ulaza za kalibraciju, za dobivanje srednje vrijednosti

//Analogni ulazi 
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);

//Digitalni ulazi SW su oni gumbici desno (3) a JSW su gumbici kad stisnes joystick
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);
//Serijska veza sa modulom za radio komunikaciju to samo prosljeduje poruku koda koristi print na PC i tera term
Serial hc(PA_9, PA_10);
//Serial pc(PA_2, PA_15);

//Proglasio i2c pinove za prosljedivanje ekranu al mozda ces moc i prosljedit akcelometru odmah
I2C i2c(PB_7, PB_6);
//Tak je pisalo u biblioteci da se proglasava
//SSD1308 oled = SSD1308(&i2c, SSD1308_SA0);

// GPS //
/*
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 TH,PI,RO,YA,TH_def;//Throttle pitch roll yaw i TH_def, th def mos promjenit ime u nes smisleno tipa th_novi il tak nes jer ovo def nemam pojma zas sam stavio nema nikakve veze s nicim
float x1_ar,x2_ar,x3_ar,y1_ar,y2_ar,y3_ar; //privremene varijable za zbrajanje ulaza, ocitava se znaci analagni ulaz i zbroji sa prijasnjim pa se na krjau sve podjeli s broje ocitanja 
//realno bolje je da ovu liniju stavis u funkciju dolje od kalibracije posto se ti brojevi samo koriste u funkciji al nije bitno 
short mid_point=50; //to samo drzi vrijednos tkoju smatramo sredinom u nasem slucaju 50 (analogni ulaz ide od 0 do 1.0 pomnozen sa sto od 0 do 100%)
short x1_cor,x2_cor,x3_cor,y1_cor,y2_cor,y3_cor; //ovo drzi korekcijski broj
short low_us=1350; //inace 1000 al da smanjimo osjetljivost joysticka smo stavili 1350 pa ide do 1650
short multiplier_us=3; // s tim mnozimo nas izracunati analogni ulaz u nominalnom stanju 50*3 znaci 150 + 1350 =1500us posto dron prima od 1000 do 2000us s tim da
// za ove vrijednosti roll pitch yaw je 1500 nominalno di se ne dogada nista a pomak prema 2000 il 1000 oznacava zakret oko osi to si mos ili u mom radu pogled ili guglas pitch yaw roll i pogleda slike da znas sta je oko koje osi
//ako je pozitivni smjer x osi u smjeru gibanja drona onda ti je yaw zakret oko Z osi  znaci dron se okrece ljevo desno, pitch je pomak oko Y dron ima nagib naprijed nazad i roll je oko x osi znaci dron ima nagib ljevo il desno

//flag za gps kad je 0 hocemo upravljat mi dronom ako se digne u 2 gps upravlja dronom
int rcom=0;
//nemam pojma sta sma tu mislio ne  koristi se za nis
//int arcom;

//gps funkcije
/*
void reads();
void rounder();
int get_deg(float);
float get_min(float);
void avg_read();
*/
//spremanje napona baterije
float vbat;
//vrijednost otpornickog djelila znaci ulazni napon  podjeljeno s tim daje napon baterije s tim da ulazni napon tj ocitanje analognog ulaza (0-1) moras prvo pomnoziti sa 3.3 
float bat_divider=0.703313;

//moras prvo proglasiti koliko je dugacak string za oled (,malo cudno), a posto je string ustvari array char to radis tako, ovjde mozemo imat 30 znakova u nasoj poruci z aoled moze s epovecat
char oled_str[30];

//prototip funkcije da ju mos pozvat u main makar je napravljena ispod maina
//kalibracija analognih ulaza
void calibration();
//skaliranje throttlea da ne ide od 1000 do 2000 nego da se postepeno penje za 10 ovisno jel drzis joystick prema gore ili dolje
void th_calc();

int main(){
    //switchevi su spojeni s jedne strane na mbed s druge na 3.3v pa ih pull downam da budu default 0 pa kad ih spojim skoce na 1
    SW1.mode(PullDown);
    SW2.mode(PullDown);
    SW3.mode(PullDown);
    JSW1.mode(PullDown);
    //naravno kinezi sposobni imaju svoj krug na joysticku za gumb i na jednom gumbu je isto tak pin spojen na mbed a drugi na VCC  il sta vec pise na toj maloj plocici od joysticka
    //ali na druga 2 joysticka je spojen na pin mbeda i na gnd pa sam moro default stavit pull up tj 1 a onda kad stisnes gumb id eu 0
    JSW2.mode(PullUp);
    JSW3.mode(PullUp);//default 1 uvjet ako 0
    //Onaj switch izmedu 2 joysticka
    TSW.mode(PullDown);
    //kalibraciju vrsim nakon sto ih sve pull up/downam
    calibration();
    //funkcije za display
    //oled.setDisplayFlip(false,false);
    //oled.setContrastControl(0xFF);
    while(1){
        //display sve
        //oled.clearDisplay();
        //sprintf(oled_str, "%d", TH_def);
        //oled.writeString(0, 0, oled_str);
        
        //ok znaci to sma ti prico vrti se petlja i ako je taj switch na 1 onda se gledaju analogni ulazi i radi joystick a ako je na 0 salje se default dronu tkao da s ene dogada nista i ne cita inpute korisnika
        if(TSW.read()){
            //gps opet
            //reads();
            //avg_read();
            //rounder();
            //ulaz znaci npr 0.54 default *100 =54- korekcija izracunata u kalibraciji pa je blizu 50 nekad 49 nekad 51 ali blize nego bez korekcije, *5=250
            //-250=0 za default nema spustanja il dizanja TH, recimo da dizes sto je u nasem slucaju zbog naopacke zalemljenog joysticka ustvari prema dolje (zato je -1 tu)
            //ond aje npr 0.74*100 -korekcija =70*5=350 -250=100 pozitivan broj ali *-1 posto smo ustvari joystick pomakli prema dolje i logicno je da je to spustanje gasa
            // pa u funkciji tj_calc vidimo d aje vrijednost negativna i dok god je negativna svakim prolazom kroz ovu petlju spustamo za 10, da je joystick prema gore bila bi pozitivna i dizali bi z a10
            TH =(-1)*((((a1.read()*100)+x1_cor)*5)-250);
            th_calc();
            // slicno kao kod th samo ovdje nam dalje pretvorbi nego se direktno vrijednost salje dronu
            //ocitano npr 0.53*100 - kalibracija =50 * multi koji je 3 znaci 150 + low us 1350 je 1500 i zbog obrnutih joysticka ovja put 3000-1500=1500
            //ali da je broj npr 1250, 3000-1250= 1750 (recimo za pitch unaprijed dron treba vise od 1500 ali mi kad taj joystick pomaknemo naprijed dobijemo manje od 1500 pa zato 3000-X nam daje suprotnu vrijednost
            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);
            //jos uvijek nez za sta sam to koristio
            //arcom=a5.read()*100;
        //svakim prolazom kroz petlju vracam rcom na 0 jer je to default i tek kad stisnes on postane nes drugo
            rcom=0;
            //switch desno u sredini sluzi za gps makar je sad na dronu iskljucen pa ga mos stiskat kolko hoces 
            if(SW2.read()==1){
                rcom=2;
            }
            //switch skroz dolje desno sluzi za  Arming drona moras mu poslat tocno te vrijednosti 
            if(SW3.read()==1){
                hc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",1000,1000,1000,1000,2000,0,0,0);
            }
            //ako nisi nista stisnuo salje se ono sa inputa normalno inace ak ne diras nes th_def je 1000 pi ro ya su 1500 rcom 0 a ove 2 nule izmedu su za longitude i latitude od gpsa
            else{
                hc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",TH_def,PI,RO,YA,1000,0,0,rcom);
            }
            //testiranje na pcu
            //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());
            
            //napon baterije mos pokazat na ekranu kao, bilo bi jebeno da sam implementiro da dron salje napon baterije svoje al jbg to nije tvoj dio ti mos na ekranu napisat Dron bat=? kao da si napravio sve d amoze primit al huda nije slao
            //vbat=(abat.read()*3.3f)/bat_divider;
            //ispis na komp
            //printf("%0.2f\n",vbat);
            //zelimo da salje sto vise ali opet ne zelimo pretjerati 10ms je ok , dron nece pola tih poruka procitat jer nema prekida na dronu za serijski ulaz pa ustvari svakim prolaz kroz
            // glavnu petlju drona ja gledam jel ima sta, ovak brzim slanjem izbjegavamo to da se mimoidu pa da dron mora bit u petlji cekat poruku jer daljinski salje tolko brzo da dron nikad ne mora cekat ustvari
            wait_ms(10);//
        
        }
        //ako switch nije ukljucen salje se default poruka da sse ne doagada nista
        else{
            
            
            hc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",1000,1500,1500,1500,2000,0,0,0);
            //to ne moramo slat tako cesto jer se ne dogada nis dron je na tlu nema letenja
            wait_ms(100);
        
        }
    }
}

void calibration(){
    //postavljane tih varijabli u 0
    x1_ar=0;
    x2_ar=0;
    x3_ar=0;
    y1_ar=0;
    y2_ar=0;
    y3_ar=0;
    //citanje ulaza zbrajanje sa prijasnjim
    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();       
    }
    //uzimamo sve zbojrene npr postose kalibracija radi dok ne mices nista recimo  je x1_ar =5.4
    // /10 mjerenja je srednja vrijednost 0.54*100=54 midpoint ili ti 50-54 je -4
    //korekcijskia vrijednost je -4 znaci da kad se gore mjeri ulaz koji mozda nece bit svaki put 54 jer je to srednja vrijednost ali kakav god da bio zbrojit ce se korekcija sa njim u ovom slucaju negativna 
    // pa ce to dat 50 lijepo il 49 il 51 mozda nekad i gore vrijednost ali u prosjeku ce bit blize 50 nego bez kalibracije
    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(){
    //ovo prvo sluzi tome da ne registriramo smetnje dok ne diramo throttle
    //jer moze vrijednost skakat malo  pa sam ovako odredio da nisu te greske vece od 10, mos stavit i 20 radi sigurnosti jer i tako joystick sada mos pomaknut skroz i bit ce isto ko kad ga pomaknes malo 
    //jer se ne gleda sama vrijednost nego jel pozitivna il negativna (osim naravno u toj prvoj provjeri za smetnje)
    if(abs(TH)<10){
        TH_def+=0;
    }
    else{
        //ako je veca povecaj th def za 10 ak je manja od 0 smanji th def za 10
        if(TH>0){
            TH_def+=10;
        }
        else{
            TH_def-=10;
        }
    }
    //sprjecavamo da ode izvan granica
    if(TH_def<1000)TH_def=1000;
    if(TH_def>2000)TH_def=2000;
    // u slucaju da osoba stisne gumb na joysticku ljevo gore th se spusta na 1000 th zaustavlaju se motori
    //sad bjaza moze pitat zas to nije prekid na sto nemam dobar odgovor jer se ne sjecam jel bilo problema kad sam stavio prekid il jednostavno nije bio potreban
    //ali mozes argumentirat da ako stisnemo gum u trenutku slanja poruke prekid moze zasmetat i mozemo poslat "losu" poruku dali ce to izazvat probleme vjerojatno ne jer ce se 10ms kasnije poslat druga poruka 
    // a i realno slanje poruka unutar prekida nije najbolje rjesenje jer se izbjegavaju radit takve stvari u prekidu  pa kad bi u prekid samo stavili ovo th_def=1000 opet bi se to tek poslalo kad bi se slala slijedeca poruka 
    //petlja se odvija toliko brzo da prekid stvarno nije potreban pogotovo kad uzmemo brzinu ljudskih refleksi u obzir tih par ms je nebitno, mislim onaj delay od 10 se moze maknut i petlja se ce odvijat maksimalnom brzinom
    //bez negativnih posljedica
    if(JSW1.read()==1)TH_def=1000;
}
//// GPS /////
/*

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

*/