Team H - TVZ Seminar / Mbed 2 deprecated Zavrsni_Daljinsk_vfinal

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed SSD1308_128x64_I2C

main.cpp@2:997bfe6f2960, 2019-09-10 (annotated)

Committer:
jjokocha
Date:
Tue Sep 10 02:21:21 2019 +0000
Revision:
2:997bfe6f2960
Parent:
1:c5bfd182c743 
Gotovo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jjokocha 1:c5bfd182c743 1 #include "mbed.h"
jjokocha 1:c5bfd182c743 2 #include "MPU9250.h"
jjokocha 1:c5bfd182c743 3 #include <math.h>
jjokocha 1:c5bfd182c743 4 #include "SSD1308.h"
jjokocha 1:c5bfd182c743 5 #define cal_num 10 // broj ocitanja analognih ulaza za kalibraciju, za dobivanje srednje vrijednosti
jjokocha 1:c5bfd182c743 6
jjokocha 1:c5bfd182c743 7 //Analogni ulazi
DudeHD 0:ac8a5576c57a 8 AnalogIn a1(PA_0);
DudeHD 0:ac8a5576c57a 9 AnalogIn a2(PA_1);
DudeHD 0:ac8a5576c57a 10 AnalogIn a3(PA_3);
DudeHD 0:ac8a5576c57a 11 AnalogIn a4(PA_4);
DudeHD 0:ac8a5576c57a 12 AnalogIn a5(PA_5);
DudeHD 0:ac8a5576c57a 13 AnalogIn a6(PA_6);
DudeHD 0:ac8a5576c57a 14 AnalogIn abat(PA_7);
jjokocha 1:c5bfd182c743 15
DudeHD 0:ac8a5576c57a 16 DigitalIn SW1(PA_11);
DudeHD 0:ac8a5576c57a 17 DigitalIn SW2(PB_5);
DudeHD 0:ac8a5576c57a 18 DigitalIn SW3(PB_4);
DudeHD 0:ac8a5576c57a 19 DigitalIn JSW1(PA_12);
DudeHD 0:ac8a5576c57a 20 DigitalIn JSW2(PB_0);
DudeHD 0:ac8a5576c57a 21 DigitalIn JSW3(PA_8);
DudeHD 0:ac8a5576c57a 22 DigitalIn TSW(PB_1);
jjokocha 2:997bfe6f2960 23 //Serijska veza sa modulom za radio komunikaciju, to samo prosljeduje poruku
DudeHD 0:ac8a5576c57a 24 Serial hc(PA_9, PA_10);
DudeHD 0:ac8a5576c57a 25 Serial pc(PA_2, PA_15);
jjokocha 1:c5bfd182c743 26
jjokocha 1:c5bfd182c743 27 //I2C i2c(PB_7, PB_6);
jjokocha 1:c5bfd182c743 28
DudeHD 0:ac8a5576c57a 29 //SSD1308 oled = SSD1308(&i2c, SSD1308_SA0);
jjokocha 1:c5bfd182c743 30
jjokocha 1:c5bfd182c743 31 short TH,PT,RO,YA,TH_def;//Throttle pitch roll yaw i TH_def
jjokocha 1:c5bfd182c743 32 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
jjokocha 1:c5bfd182c743 33 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%)
jjokocha 1:c5bfd182c743 34 short x1_cor,x2_cor,x3_cor,y1_cor,y2_cor,y3_cor; // korekcijski broj
jjokocha 1:c5bfd182c743 35 short low_us=1350; //inace 1000 al da smanjimo osjetljivost joysticka smo stavili 1350 pa ide do 1650
jjokocha 2:997bfe6f2960 36 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
jjokocha 1:c5bfd182c743 37 // za ove vrijednosti roll pitch yaw je 1500 nominalno di se ne dogada nista a pomak prema 2000 il 1000 oznacava zakret oko osi
jjokocha 1:c5bfd182c743 38
DudeHD 0:ac8a5576c57a 39 int rcom=0;
jjokocha 1:c5bfd182c743 40
jjokocha 1:c5bfd182c743 41 //spremanje napona baterije
DudeHD 0:ac8a5576c57a 42 float vbat;
jjokocha 1:c5bfd182c743 43 //vrijednost otpornickog djelila znaci ulazni napon podjeljeno s tim daje napon baterije s tim da ulazni napon tj ocitanje analognog ulaza (0-1)* 3.3
DudeHD 0:ac8a5576c57a 44 float bat_divider=0.703313;
DudeHD 0:ac8a5576c57a 45 char oled_str[30];
DudeHD 0:ac8a5576c57a 46 void calibration();
DudeHD 0:ac8a5576c57a 47 void th_calc();
jjokocha 1:c5bfd182c743 48
jjokocha 1:c5bfd182c743 49 //Senzor
jjokocha 1:c5bfd182c743 50
jjokocha 2:997bfe6f2960 51 float sum = 0;
jjokocha 1:c5bfd182c743 52 uint32_t sumCount = 0;
jjokocha 1:c5bfd182c743 53
jjokocha 1:c5bfd182c743 54 MPU9250 mpu9250;
jjokocha 1:c5bfd182c743 55
jjokocha 1:c5bfd182c743 56 Timer t;
jjokocha 1:c5bfd182c743 57
jjokocha 2:997bfe6f2960 58 volatile bool newData = false;
jjokocha 1:c5bfd182c743 59
jjokocha 1:c5bfd182c743 60 InterruptIn isrPin(D12); //k64 D12 dragon PD_0
jjokocha 1:c5bfd182c743 61
jjokocha 1:c5bfd182c743 62 void mpuisr()
jjokocha 1:c5bfd182c743 63 {
jjokocha 1:c5bfd182c743 64 newData=true;
jjokocha 1:c5bfd182c743 65 }
jjokocha 1:c5bfd182c743 66
jjokocha 1:c5bfd182c743 67
jjokocha 1:c5bfd182c743 68
DudeHD 0:ac8a5576c57a 69 int main(){
jjokocha 1:c5bfd182c743 70
DudeHD 0:ac8a5576c57a 71 SW1.mode(PullDown);
DudeHD 0:ac8a5576c57a 72 SW2.mode(PullDown);
DudeHD 0:ac8a5576c57a 73 SW3.mode(PullDown);
DudeHD 0:ac8a5576c57a 74 JSW1.mode(PullDown);
DudeHD 0:ac8a5576c57a 75 JSW2.mode(PullUp);
jjokocha 1:c5bfd182c743 76 JSW3.mode(PullUp);
jjokocha 1:c5bfd182c743 77 //switch izmedu 2 joysticka
DudeHD 0:ac8a5576c57a 78 TSW.mode(PullDown);
jjokocha 1:c5bfd182c743 79 //kalibraciju vrsim nakon sto ih sve pull up/downam
DudeHD 0:ac8a5576c57a 80 calibration();
jjokocha 1:c5bfd182c743 81 //funkcije za display
DudeHD 0:ac8a5576c57a 82 //oled.setDisplayFlip(false,false);
DudeHD 0:ac8a5576c57a 83 //oled.setContrastControl(0xFF);
jjokocha 1:c5bfd182c743 84
jjokocha 2:997bfe6f2960 85
jjokocha 1:c5bfd182c743 86 //Senzor
jjokocha 1:c5bfd182c743 87
jjokocha 1:c5bfd182c743 88 //pc.baud(9600);
jjokocha 1:c5bfd182c743 89
jjokocha 1:c5bfd182c743 90 i2c.frequency(400000); // use fast (400 kHz) I2C
jjokocha 1:c5bfd182c743 91 t.start();
jjokocha 1:c5bfd182c743 92 isrPin.rise(&mpuisr);
jjokocha 1:c5bfd182c743 93
jjokocha 1:c5bfd182c743 94 // Read the WHO_AM_I register, this is a good test of communication
jjokocha 1:c5bfd182c743 95 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
jjokocha 2:997bfe6f2960 96
jjokocha 1:c5bfd182c743 97
jjokocha 1:c5bfd182c743 98 if (whoami == 0x71) { // WHO_AM_I should always be 0x68
jjokocha 1:c5bfd182c743 99 pc.printf("MPU9250 is online...\n\r");
jjokocha 1:c5bfd182c743 100 wait(1);
jjokocha 1:c5bfd182c743 101
jjokocha 1:c5bfd182c743 102
jjokocha 1:c5bfd182c743 103 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
jjokocha 1:c5bfd182c743 104 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
jjokocha 1:c5bfd182c743 105
jjokocha 1:c5bfd182c743 106 wait(2);
jjokocha 1:c5bfd182c743 107 mpu9250.initMPU9250();
jjokocha 1:c5bfd182c743 108 mpu9250.initAK8963(magCalibration);
jjokocha 1:c5bfd182c743 109 if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
jjokocha 1:c5bfd182c743 110 if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
jjokocha 1:c5bfd182c743 111 if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
jjokocha 1:c5bfd182c743 112 if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
jjokocha 1:c5bfd182c743 113 wait(2);
jjokocha 1:c5bfd182c743 114 } else {
jjokocha 1:c5bfd182c743 115 pc.printf("Could not connect to MPU9250: \n\r");
jjokocha 1:c5bfd182c743 116 pc.printf("%#x \n", whoami);
jjokocha 1:c5bfd182c743 117
jjokocha 1:c5bfd182c743 118
jjokocha 1:c5bfd182c743 119 while(1) ; // Loop forever if communication doesn't happen
jjokocha 1:c5bfd182c743 120 }
jjokocha 1:c5bfd182c743 121
jjokocha 1:c5bfd182c743 122 mpu9250.getAres(); // Get accelerometer sensitivity
jjokocha 1:c5bfd182c743 123 mpu9250.getGres(); // Get gyro sensitivity
jjokocha 1:c5bfd182c743 124 mpu9250.getMres(); // Get magnetometer sensitivity
jjokocha 1:c5bfd182c743 125 pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
jjokocha 1:c5bfd182c743 126 pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
jjokocha 1:c5bfd182c743 127 pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
jjokocha 1:c5bfd182c743 128 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
jjokocha 1:c5bfd182c743 129 magbias[1] = +120.; // User environmental x-axis correction in milliGauss
jjokocha 1:c5bfd182c743 130 magbias[2] = +125.; // User environmental x-axis correction in milliGauss
jjokocha 2:997bfe6f2960 131
jjokocha 1:c5bfd182c743 132
jjokocha 1:c5bfd182c743 133
DudeHD 0:ac8a5576c57a 134 while(1){
jjokocha 1:c5bfd182c743 135 //display sve
DudeHD 0:ac8a5576c57a 136 //oled.clearDisplay();
jjokocha 1:c5bfd182c743 137 //sprintf(oled_str, "%d", TH_def);
DudeHD 0:ac8a5576c57a 138 //oled.writeString(0, 0, oled_str);
jjokocha 1:c5bfd182c743 139
DudeHD 0:ac8a5576c57a 140 if(TSW.read()){
jjokocha 2:997bfe6f2960 141
DudeHD 0:ac8a5576c57a 142 TH =(-1)*((((a1.read()*100)+x1_cor)*5)-250);
DudeHD 0:ac8a5576c57a 143 th_calc();
jjokocha 2:997bfe6f2960 144
jjokocha 1:c5bfd182c743 145 PT =3000-(low_us+((a3.read()*100)+y1_cor)*multiplier_us);
DudeHD 0:ac8a5576c57a 146 YA =3000-(low_us+((a2.read()*100)+x2_cor)*multiplier_us);
jjokocha 1:c5bfd182c743 147 RO =3000-(low_us+((a4.read()*100)+y2_cor)*multiplier_us);
jjokocha 1:c5bfd182c743 148
DudeHD 0:ac8a5576c57a 149 rcom=0;
jjokocha 1:c5bfd182c743 150 //switch desno u sredini sluzi za gps
DudeHD 0:ac8a5576c57a 151 if(SW2.read()==1){
DudeHD 0:ac8a5576c57a 152 rcom=2;
DudeHD 0:ac8a5576c57a 153 }
jjokocha 1:c5bfd182c743 154 //switch skroz dolje desno sluzi za Arming drona
DudeHD 0:ac8a5576c57a 155 if(SW3.read()==1){
DudeHD 0:ac8a5576c57a 156 hc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",1000,1000,1000,1000,2000,0,0,0);
DudeHD 0:ac8a5576c57a 157 }
jjokocha 1:c5bfd182c743 158
DudeHD 0:ac8a5576c57a 159 else{
jjokocha 1:c5bfd182c743 160 hc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",TH_def,RO,PT,YA,1000,0,0,rcom);
jjokocha 1:c5bfd182c743 161 pc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",TH_def,RO,PT,YA,1000,0,0,rcom);
jjokocha 1:c5bfd182c743 162
DudeHD 0:ac8a5576c57a 163 }
jjokocha 2:997bfe6f2960 164
DudeHD 0:ac8a5576c57a 165
jjokocha 1:c5bfd182c743 166 vbat=(abat.read()*3.3f)/bat_divider;
jjokocha 1:c5bfd182c743 167 pc.printf("%0.2f\n",vbat);
jjokocha 1:c5bfd182c743 168 wait_ms(10);//
DudeHD 0:ac8a5576c57a 169
DudeHD 0:ac8a5576c57a 170 }
jjokocha 1:c5bfd182c743 171
DudeHD 0:ac8a5576c57a 172 else{
jjokocha 1:c5bfd182c743 173
jjokocha 1:c5bfd182c743 174 TH =(-1)*((((a1.read()*100)+x1_cor)*5)-250);
jjokocha 1:c5bfd182c743 175 th_calc();
DudeHD 0:ac8a5576c57a 176
jjokocha 2:997bfe6f2960 177 static int readycnt=0;
jjokocha 1:c5bfd182c743 178 // If intPin goes high, all data registers have new data
jjokocha 1:c5bfd182c743 179
jjokocha 1:c5bfd182c743 180 #if USE_ISR
jjokocha 1:c5bfd182c743 181 if(newData) {
jjokocha 1:c5bfd182c743 182 newData=false;
jjokocha 1:c5bfd182c743 183 mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS); //? need this with ISR
jjokocha 1:c5bfd182c743 184 #else
jjokocha 1:c5bfd182c743 185 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
jjokocha 1:c5bfd182c743 186 #endif
jjokocha 1:c5bfd182c743 187 readycnt++;
jjokocha 1:c5bfd182c743 188 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
jjokocha 1:c5bfd182c743 189 // Now we'll calculate the accleration value into actual g's
jjokocha 1:c5bfd182c743 190 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
jjokocha 1:c5bfd182c743 191 ay = (float)accelCount[1]*aRes - accelBias[1];
jjokocha 1:c5bfd182c743 192 az = (float)accelCount[2]*aRes - accelBias[2];
jjokocha 1:c5bfd182c743 193
jjokocha 1:c5bfd182c743 194 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
jjokocha 1:c5bfd182c743 195 // Calculate the gyro value into actual degrees per second
jjokocha 1:c5bfd182c743 196 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
jjokocha 1:c5bfd182c743 197 gy = (float)gyroCount[1]*gRes - gyroBias[1];
jjokocha 1:c5bfd182c743 198 gz = (float)gyroCount[2]*gRes - gyroBias[2];
jjokocha 1:c5bfd182c743 199
jjokocha 1:c5bfd182c743 200 mpu9250.readMagData(magCount); // Read the x/y/z adc values
jjokocha 1:c5bfd182c743 201 // Calculate the magnetometer values in milliGauss
jjokocha 1:c5bfd182c743 202 // Include factory calibration per data sheet and user environmental corrections
jjokocha 1:c5bfd182c743 203 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
jjokocha 1:c5bfd182c743 204 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
jjokocha 1:c5bfd182c743 205 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
jjokocha 1:c5bfd182c743 206 }
jjokocha 1:c5bfd182c743 207
jjokocha 1:c5bfd182c743 208 Now = t.read_us();
jjokocha 1:c5bfd182c743 209 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
jjokocha 1:c5bfd182c743 210 lastUpdate = Now;
jjokocha 1:c5bfd182c743 211
jjokocha 1:c5bfd182c743 212 sum += deltat;
jjokocha 1:c5bfd182c743 213 sumCount++;
jjokocha 1:c5bfd182c743 214
jjokocha 1:c5bfd182c743 215
jjokocha 2:997bfe6f2960 216
jjokocha 1:c5bfd182c743 217 uint32_t us = t.read_us();
jjokocha 1:c5bfd182c743 218 mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
jjokocha 1:c5bfd182c743 219 us = t.read_us()-us;
jjokocha 1:c5bfd182c743 220 int delt_t = 0; // used to control display output rate
jjokocha 1:c5bfd182c743 221 int count = 0; // used to control display output rate
jjokocha 2:997bfe6f2960 222
jjokocha 1:c5bfd182c743 223 delt_t = t.read_ms() - count;
jjokocha 2:997bfe6f2960 224
jjokocha 1:c5bfd182c743 225 readycnt=0;
jjokocha 1:c5bfd182c743 226
jjokocha 1:c5bfd182c743 227 tempCount = mpu9250.readTempData(); // Read the adc values
jjokocha 1:c5bfd182c743 228 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
jjokocha 1:c5bfd182c743 229
jjokocha 1:c5bfd182c743 230 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]);
jjokocha 1:c5bfd182c743 231 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
jjokocha 1:c5bfd182c743 232 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]);
jjokocha 1:c5bfd182c743 233 pitch *= 180.0f / PI;
jjokocha 1:c5bfd182c743 234 yaw *= 180.0f / PI;
jjokocha 2:997bfe6f2960 235 yaw += 3.95f; // Godine 2018 magnetna deklinacija je +3* 57’ za Zagreb
jjokocha 1:c5bfd182c743 236 roll *= 180.0f / PI;
jjokocha 2:997bfe6f2960 237
jjokocha 2:997bfe6f2960 238 if ( pitch < 10 && pitch > -20) {
jjokocha 2:997bfe6f2960 239 PT = 1500;
jjokocha 2:997bfe6f2960 240 }
jjokocha 2:997bfe6f2960 241 else {
jjokocha 2:997bfe6f2960 242
jjokocha 2:997bfe6f2960 243 if ( pitch > 10 ) {
jjokocha 2:997bfe6f2960 244 PT = 1500 + ((pitch - 10 )*3.75);
jjokocha 2:997bfe6f2960 245 }
jjokocha 2:997bfe6f2960 246 else if (pitch < -20 ){
jjokocha 2:997bfe6f2960 247 PT = 1500 + (( pitch + 20)*3.75);
jjokocha 2:997bfe6f2960 248 }
jjokocha 2:997bfe6f2960 249 }
jjokocha 2:997bfe6f2960 250 if(pitch < -60)PT=1350;
jjokocha 2:997bfe6f2960 251 if(pitch > 50)PT=1650;
jjokocha 2:997bfe6f2960 252
jjokocha 2:997bfe6f2960 253 if ( abs(roll)<10) {
jjokocha 2:997bfe6f2960 254 RO = 1500;
jjokocha 2:997bfe6f2960 255 }
jjokocha 2:997bfe6f2960 256 else {
jjokocha 2:997bfe6f2960 257
jjokocha 2:997bfe6f2960 258 if ( roll > 10 ) {
jjokocha 2:997bfe6f2960 259 RO = 1500 + ((roll - 10 )*3.75);
jjokocha 2:997bfe6f2960 260 }
jjokocha 2:997bfe6f2960 261 else if (roll < -10 ){
jjokocha 2:997bfe6f2960 262 RO = 1500 + (( roll + 10)*3.75);
jjokocha 2:997bfe6f2960 263 }
jjokocha 2:997bfe6f2960 264 }
jjokocha 2:997bfe6f2960 265 if(roll < -50)RO=1350;
jjokocha 2:997bfe6f2960 266 if(roll > 50)RO=1650;
jjokocha 2:997bfe6f2960 267
jjokocha 2:997bfe6f2960 268 YA =3000-(low_us+((a2.read()*100)+x2_cor)*multiplier_us);
jjokocha 2:997bfe6f2960 269
jjokocha 2:997bfe6f2960 270
jjokocha 2:997bfe6f2960 271 hc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",TH_def,RO,PT,YA,1000,0,0,rcom);
jjokocha 2:997bfe6f2960 272 //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
jjokocha 2:997bfe6f2960 273
jjokocha 1:c5bfd182c743 274
jjokocha 1:c5bfd182c743 275 count = t.read_ms();
jjokocha 1:c5bfd182c743 276 sum = 0;
jjokocha 1:c5bfd182c743 277 sumCount = 0;
jjokocha 1:c5bfd182c743 278
jjokocha 1:c5bfd182c743 279
DudeHD 0:ac8a5576c57a 280
DudeHD 0:ac8a5576c57a 281 }
DudeHD 0:ac8a5576c57a 282 }
DudeHD 0:ac8a5576c57a 283 }
jjokocha 1:c5bfd182c743 284
DudeHD 0:ac8a5576c57a 285 void calibration(){
jjokocha 1:c5bfd182c743 286
DudeHD 0:ac8a5576c57a 287 x1_ar=0;
DudeHD 0:ac8a5576c57a 288 x2_ar=0;
DudeHD 0:ac8a5576c57a 289 x3_ar=0;
DudeHD 0:ac8a5576c57a 290 y1_ar=0;
DudeHD 0:ac8a5576c57a 291 y2_ar=0;
DudeHD 0:ac8a5576c57a 292 y3_ar=0;
jjokocha 1:c5bfd182c743 293
DudeHD 0:ac8a5576c57a 294 for(int a=0;a<cal_num;a++){
DudeHD 0:ac8a5576c57a 295 x1_ar+=a1.read();
DudeHD 0:ac8a5576c57a 296 x2_ar+=a2.read();
DudeHD 0:ac8a5576c57a 297 x3_ar+=a5.read();
DudeHD 0:ac8a5576c57a 298 y1_ar+=a3.read();
DudeHD 0:ac8a5576c57a 299 y2_ar+=a4.read();
DudeHD 0:ac8a5576c57a 300 y3_ar+=a6.read();
DudeHD 0:ac8a5576c57a 301 }
DudeHD 0:ac8a5576c57a 302 x1_cor=mid_point-((x1_ar/cal_num)*100);
DudeHD 0:ac8a5576c57a 303 x2_cor=mid_point-((x2_ar/cal_num)*100);
DudeHD 0:ac8a5576c57a 304 x3_cor=mid_point-((x3_ar/cal_num)*100);
DudeHD 0:ac8a5576c57a 305 y1_cor=mid_point-((y1_ar/cal_num)*100);
DudeHD 0:ac8a5576c57a 306 y2_cor=mid_point-((y2_ar/cal_num)*100);
DudeHD 0:ac8a5576c57a 307 y3_cor=mid_point-((y3_ar/cal_num)*100);
jjokocha 2:997bfe6f2960 308
DudeHD 0:ac8a5576c57a 309 }
jjokocha 1:c5bfd182c743 310
DudeHD 0:ac8a5576c57a 311 void th_calc(){
jjokocha 1:c5bfd182c743 312
DudeHD 0:ac8a5576c57a 313 if(abs(TH)<10){
DudeHD 0:ac8a5576c57a 314 TH_def+=0;
DudeHD 0:ac8a5576c57a 315 }
DudeHD 0:ac8a5576c57a 316 else{
jjokocha 2:997bfe6f2960 317
DudeHD 0:ac8a5576c57a 318 if(TH>0){
DudeHD 0:ac8a5576c57a 319 TH_def+=10;
DudeHD 0:ac8a5576c57a 320 }
DudeHD 0:ac8a5576c57a 321 else{
DudeHD 0:ac8a5576c57a 322 TH_def-=10;
DudeHD 0:ac8a5576c57a 323 }
DudeHD 0:ac8a5576c57a 324 }
jjokocha 2:997bfe6f2960 325
DudeHD 0:ac8a5576c57a 326 if(TH_def<1000)TH_def=1000;
jjokocha 1:c5bfd182c743 327 if(TH_def>2000)TH_def=2000;
jjokocha 2:997bfe6f2960 328 if(JSW2.read()==0)TH_def=1000;
jjokocha 1:c5bfd182c743 329 }
jjokocha 2:997bfe6f2960 330
jjokocha 2:997bfe6f2960 331
jjokocha 2:997bfe6f2960 332
jjokocha 2:997bfe6f2960 333
jjokocha 2:997bfe6f2960 334
jjokocha 2:997bfe6f2960 335
jjokocha 1:c5bfd182c743 336
jjokocha 1:c5bfd182c743 337
jjokocha 1:c5bfd182c743 338
jjokocha 1:c5bfd182c743 339
jjokocha 1:c5bfd182c743 340
jjokocha 1:c5bfd182c743 341
jjokocha 1:c5bfd182c743 342
jjokocha 1:c5bfd182c743 343