Revision 2:997bfe6f2960, committed 2019-09-10

Comitter:

jjokocha

Date:

Tue Sep 10 02:21:21 2019 +0000

Parent:

1:c5bfd182c743

Commit message:

Gotovo

Changed in this revision

diff -r c5bfd182c743 -r 997bfe6f2960 main.cpp
--- a/main.cpp	Sat Sep 07 17:51:07 2019 +0000
+++ b/main.cpp	Tue Sep 10 02:21:21 2019 +0000
@@ -20,7 +20,7 @@
 DigitalIn JSW2(PB_0);
 DigitalIn JSW3(PA_8);
 DigitalIn TSW(PB_1);
-//Serijska veza sa modulom za radio komunikaciju to samo prosljeduje poruku
+//Serijska veza sa modulom za radio komunikaciju, to samo prosljeduje poruku
 Serial hc(PA_9, PA_10);
 Serial pc(PA_2, PA_15);
  
@@ -33,7 +33,7 @@
 short mid_point=50; //vrijednost koju 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; // 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
+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
 // za ove vrijednosti roll pitch yaw je 1500 nominalno di se ne dogada nista a pomak prema 2000 il 1000 oznacava zakret oko osi 
  
 int rcom=0;
@@ -46,17 +46,16 @@
 void calibration();
 void th_calc();
  
- 
  //Senzor
  
- float sum = 0;
+float sum = 0;
 uint32_t sumCount = 0;
  
 MPU9250 mpu9250;
  
 Timer t;
  
- volatile bool newData = false;
+volatile bool newData = false;
  
 InterruptIn isrPin(D12);   //k64 D12  dragon PD_0
  
@@ -83,23 +82,18 @@
     //oled.setDisplayFlip(false,false);
     //oled.setContrastControl(0xFF);
     
-    
+ 
     //Senzor
     
         //pc.baud(9600);
  
-    //Set up I2C
     i2c.frequency(400000);  // use fast (400 kHz) I2C
- 
-    //pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
- 
     t.start();
     isrPin.rise(&mpuisr);
  
     // Read the WHO_AM_I register, this is a good test of communication
     uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
-    //pc.printf("I AM 0x%x\n\r", whoami);
-    //pc.printf("I SHOULD BE 0x71\n\r");
+    
  
     if (whoami == 0x71) { // WHO_AM_I should always be 0x68
         pc.printf("MPU9250 is online...\n\r");
@@ -134,6 +128,7 @@
     magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
     magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
     magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
+   
     
     
     while(1){
@@ -143,15 +138,10 @@
         //oled.writeString(0, 0, oled_str);
         
         if(TSW.read()){
-            //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
+            
             PT =3000-(low_us+((a3.read()*100)+y1_cor)*multiplier_us); 
             YA =3000-(low_us+((a2.read()*100)+x2_cor)*multiplier_us);
             RO =3000-(low_us+((a4.read()*100)+y2_cor)*multiplier_us);
@@ -171,12 +161,9 @@
                 pc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",TH_def,RO,PT,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());
+           
             
             vbat=(abat.read()*3.3f)/bat_divider;
-            //ispis na komp
             pc.printf("%0.2f\n",vbat);
             wait_ms(10);//
         
@@ -187,7 +174,7 @@
             TH =(-1)*((((a1.read()*100)+x1_cor)*5)-250);
             th_calc();
             
-                    static int readycnt=0;
+                   static int readycnt=0;
         // If intPin goes high, all data registers have new data
  
 #if USE_ISR
@@ -226,46 +213,64 @@
         sumCount++;
  
  
-        // Pass gyro rate as rad/s
+        
         uint32_t us = t.read_us();
         mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
         us = t.read_us()-us;
         int delt_t = 0; // used to control display output rate
         int count = 0;  // used to control display output rate
-        // Serial print and/or display at 0.5 s rate independent of data rates
+        
         delt_t = t.read_ms() - count;
-            //pc.printf("readycnt %d us %d\n",readycnt,us);
+            
             readycnt=0;
-           // pc.printf("ax = %f", 1000*ax);
-           // pc.printf(" ay = %f", 1000*ay);
-           // pc.printf(" az = %f  mg\n\r", 1000*az);
- 
-          //  pc.printf("gx = %f", gx);
-          //  pc.printf(" gy = %f", gy);
-           // pc.printf(" gz = %f  deg/s\n\r", gz);
- 
-           // pc.printf("gx = %f", mx);
-           // pc.printf(" gy = %f", my);
-           // pc.printf(" gz = %f  mG\n\r", mz);
  
             tempCount = mpu9250.readTempData();  // Read the adc values
             temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
-           // pc.printf("temperature = %f  C\n\r", temperature);
- 
-           // pc.printf("q0 = %f\n\r", q[0]);
-           // pc.printf("q1 = %f\n\r", q[1]);
-          //  pc.printf("q2 = %f\n\r", q[2]);
-          //  pc.printf("q3 = %f\n\r", q[3]);
             
             yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
             pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
             roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
             pitch *= 180.0f / PI;
             yaw   *= 180.0f / PI;
-            yaw   += 3.95f; // Godine 2018 magnetna deklinacija je +3* 57’ za Zagreb
+            yaw   += 3.95f;   // Godine 2018 magnetna deklinacija je +3* 57’ za Zagreb
             roll  *= 180.0f / PI;
- 
-            pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
+            
+            if ( pitch < 10 && pitch > -20) {
+                PT = 1500;
+            }
+            else {
+        
+                if ( pitch > 10 ) {
+                    PT = 1500 + ((pitch - 10 )*3.75);
+                    }
+                else if (pitch < -20 ){
+                    PT = 1500 + (( pitch + 20)*3.75);
+                    }
+                  }
+            if(pitch < -60)PT=1350;
+            if(pitch > 50)PT=1650;
+            
+            if ( abs(roll)<10) {
+                RO = 1500;
+                }
+            else {
+        
+                if ( roll > 10 ) {
+                    RO = 1500 + ((roll - 10 )*3.75);
+                    }
+                else if (roll < -10 ){
+                    RO = 1500 + (( roll + 10)*3.75);
+                    }
+                  }
+            if(roll < -50)RO=1350;
+            if(roll > 50)RO=1650;
+            
+            YA =3000-(low_us+((a2.read()*100)+x2_cor)*multiplier_us);
+            
+            
+            hc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",TH_def,RO,PT,YA,1000,0,0,rcom);
+            //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
+         
            
             count = t.read_ms();
             sum = 0;
@@ -300,7 +305,7 @@
     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(){
@@ -309,7 +314,7 @@
         TH_def+=0;
     }
     else{
-        //ako je veca povecaj th def za 10, ako je manja od 0 smanji th def za 10
+    
         if(TH>0){
             TH_def+=10;
         }
@@ -317,12 +322,17 @@
             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 lijevo th se spusta na 1000 th zaustavlaju se motori
-    if(JSW2.read()==1)TH_def=1000;
+    if(JSW2.read()==0)TH_def=1000;
 }
+
+
+    
+    
+             
+