Diff: functions.cpp

Revision:

0:3dc012104243

Child:

4:9ba47e5db1e2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/functions.cpp	Tue Sep 24 13:35:31 2019 +0000
@@ -0,0 +1,140 @@
+#include "mbed.h"
+#include "functions.h"
+
+const int addr = 0x68 << 1;
+const int Sampling = 4000;
+
+I2C i2c(PB_11 , PB_10);//PB_11 -> PB_9
+                       //PB_10 -> PB_8
+                         //PF_0,PF_1
+Timer timer;
+Serial ss(USBTX,USBRX);
+int elapsed_time;
+char funcd[2];
+double offset_gz = 0; 
+int error_c = 0;
+
+void readGyz(double* gz){
+    int16_t a = i2c_read_16bit(addr,0x48,0x47);
+    //ss.printf("%d\n",a);
+    *gz = a*0.5;
+    //0.01526717
+    *gz *= 0.01527;
+    elapsed_time = timer.read_ms();
+    timer.reset();
+    }
+
+void mpu_setup(){
+    i2c.frequency(400000);
+    timer.start();
+    i2c_write(addr,0x6b,0x00);
+    i2c_write(addr,0x37,0x02);
+    i2c_write(addr,0x27,0b1000);
+    }
+
+void get_offset(){
+    for(int i =0;i<Sampling;i++){
+        
+        double yaw;
+        readGyz(&yaw);
+        if(yaw == 139.323480)error_c++;
+        //ss.printf("%f\n",yaw);
+        offset_gz += yaw;
+        }
+        offset_gz /= Sampling;
+    }
+
+int rez(){
+    return(error_c);
+    }
+    
+void to_signed(double* all){
+        if(*all < 0){
+          *all += 360;
+        }
+        if(*all >= 360){
+          *all -= 360;
+        }
+        
+        if(*all >= 180){
+          *all -= 360;
+        }
+    }
+
+char *puc(double all){
+    int a = int(all);
+    if(a > 999){
+        char st[4];
+        int b = (int)a/1000;
+        int c = int((a-b*1000)/100);
+        int d = int((a-b*1000-c*100)/10);
+        int e = a-b*1000-c*100-d*10;
+        st[0] = ('0'+b);
+        st[1] = ('0'+c);
+        st[2] = ('0'+d);
+        st[3] = ('0'+e);
+        return(st);
+    }else if(a > 99){//3桁
+        char st[3];
+        int b = (int)a/100;
+        int c = int((a-b*100)/10);
+        int d = int(a-b*100-c*10);
+        st[0] = ('0'+b);
+        st[1] = ('0'+c);
+        st[2] = ('0'+d);
+        return(st);
+    }else if(a > 9){
+        char st[2];
+        int b = (int)a/10;
+        int c = int(a-b*10);
+        st[0] ='0'+b;
+        st[1] ='0'+c;
+        return(st);
+    }else{
+        char st[1];
+        st[0] = '0'+a;
+        return(st);
+        }
+    }
+void i2c_write(int addr,char regist,char data)
+{   
+    funcd[0]=regist;
+    funcd[1]=data;
+    i2c.write(addr,funcd,2);
+}
+ 
+char i2c_read(int addr,char regist)
+{
+    funcd[0]=regist;
+    i2c.write(addr,funcd,1);
+    i2c.read(addr,funcd,1);
+    return funcd[0];
+}
+ 
+int i2c_read_16bit(int addr,char registL,char registH)
+{
+    return (int(i2c_read(addr,registH))<<8)+int(i2c_read(addr,registL));
+}
+
+void offset_adjust(double* all,double* gz){
+    if(!(*gz-offset_gz < 1 && *gz-offset_gz > -1)){
+            *all += (*gz-offset_gz)*elapsed_time*0.001;
+    }
+}
+
+float angle_adjust(float Gein,double all,float Min_ang,float Min_pwm,float Max_pwm_roll){
+        float pwm;
+        if(all > 0){
+            pwm = all*Gein;
+        }else{
+            pwm = -all*Gein;
+                }
+        if(all < Min_ang && all > -Min_ang){
+            pwm = 0;
+        }
+        if(pwm < Min_pwm)pwm = 0;
+        if(pwm > Max_pwm_roll){
+            pwm = Max_pwm_roll;
+            }
+    return(pwm);
+    }
\ No newline at end of file