Ryu Kaz / Mbed OS 767zi

t6est

Dependencies:   Pulse

functions.cpp@0:3dc012104243, 2019-09-24 (annotated)

Committer:
kazuryu
Date:
Tue Sep 24 13:35:31 2019 +0000
Revision:
0:3dc012104243
Child:
4:9ba47e5db1e2
test;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kazuryu 0:3dc012104243 1 #include "mbed.h"
kazuryu 0:3dc012104243 2 #include "functions.h"
kazuryu 0:3dc012104243 3
kazuryu 0:3dc012104243 4 const int addr = 0x68 << 1;
kazuryu 0:3dc012104243 5 const int Sampling = 4000;
kazuryu 0:3dc012104243 6
kazuryu 0:3dc012104243 7 I2C i2c(PB_11 , PB_10);//PB_11 -> PB_9
kazuryu 0:3dc012104243 8 //PB_10 -> PB_8
kazuryu 0:3dc012104243 9 //PF_0,PF_1
kazuryu 0:3dc012104243 10 Timer timer;
kazuryu 0:3dc012104243 11 Serial ss(USBTX,USBRX);
kazuryu 0:3dc012104243 12 int elapsed_time;
kazuryu 0:3dc012104243 13 char funcd[2];
kazuryu 0:3dc012104243 14 double offset_gz = 0;
kazuryu 0:3dc012104243 15 int error_c = 0;
kazuryu 0:3dc012104243 16
kazuryu 0:3dc012104243 17 void readGyz(double* gz){
kazuryu 0:3dc012104243 18 int16_t a = i2c_read_16bit(addr,0x48,0x47);
kazuryu 0:3dc012104243 19 //ss.printf("%d\n",a);
kazuryu 0:3dc012104243 20 *gz = a*0.5;
kazuryu 0:3dc012104243 21 //0.01526717
kazuryu 0:3dc012104243 22 *gz *= 0.01527;
kazuryu 0:3dc012104243 23 elapsed_time = timer.read_ms();
kazuryu 0:3dc012104243 24 timer.reset();
kazuryu 0:3dc012104243 25 }
kazuryu 0:3dc012104243 26
kazuryu 0:3dc012104243 27 void mpu_setup(){
kazuryu 0:3dc012104243 28 i2c.frequency(400000);
kazuryu 0:3dc012104243 29 timer.start();
kazuryu 0:3dc012104243 30 i2c_write(addr,0x6b,0x00);
kazuryu 0:3dc012104243 31 i2c_write(addr,0x37,0x02);
kazuryu 0:3dc012104243 32 i2c_write(addr,0x27,0b1000);
kazuryu 0:3dc012104243 33 }
kazuryu 0:3dc012104243 34
kazuryu 0:3dc012104243 35 void get_offset(){
kazuryu 0:3dc012104243 36 for(int i =0;i<Sampling;i++){
kazuryu 0:3dc012104243 37
kazuryu 0:3dc012104243 38 double yaw;
kazuryu 0:3dc012104243 39 readGyz(&yaw);
kazuryu 0:3dc012104243 40 if(yaw == 139.323480)error_c++;
kazuryu 0:3dc012104243 41 //ss.printf("%f\n",yaw);
kazuryu 0:3dc012104243 42 offset_gz += yaw;
kazuryu 0:3dc012104243 43 }
kazuryu 0:3dc012104243 44 offset_gz /= Sampling;
kazuryu 0:3dc012104243 45 }
kazuryu 0:3dc012104243 46
kazuryu 0:3dc012104243 47 int rez(){
kazuryu 0:3dc012104243 48 return(error_c);
kazuryu 0:3dc012104243 49 }
kazuryu 0:3dc012104243 50
kazuryu 0:3dc012104243 51 void to_signed(double* all){
kazuryu 0:3dc012104243 52 if(*all < 0){
kazuryu 0:3dc012104243 53 *all += 360;
kazuryu 0:3dc012104243 54 }
kazuryu 0:3dc012104243 55 if(*all >= 360){
kazuryu 0:3dc012104243 56 *all -= 360;
kazuryu 0:3dc012104243 57 }
kazuryu 0:3dc012104243 58
kazuryu 0:3dc012104243 59 if(*all >= 180){
kazuryu 0:3dc012104243 60 *all -= 360;
kazuryu 0:3dc012104243 61 }
kazuryu 0:3dc012104243 62 }
kazuryu 0:3dc012104243 63
kazuryu 0:3dc012104243 64 char *puc(double all){
kazuryu 0:3dc012104243 65 int a = int(all);
kazuryu 0:3dc012104243 66 if(a > 999){
kazuryu 0:3dc012104243 67 char st[4];
kazuryu 0:3dc012104243 68 int b = (int)a/1000;
kazuryu 0:3dc012104243 69 int c = int((a-b*1000)/100);
kazuryu 0:3dc012104243 70 int d = int((a-b*1000-c*100)/10);
kazuryu 0:3dc012104243 71 int e = a-b*1000-c*100-d*10;
kazuryu 0:3dc012104243 72 st[0] = ('0'+b);
kazuryu 0:3dc012104243 73 st[1] = ('0'+c);
kazuryu 0:3dc012104243 74 st[2] = ('0'+d);
kazuryu 0:3dc012104243 75 st[3] = ('0'+e);
kazuryu 0:3dc012104243 76 return(st);
kazuryu 0:3dc012104243 77 }else if(a > 99){//3桁
kazuryu 0:3dc012104243 78 char st[3];
kazuryu 0:3dc012104243 79 int b = (int)a/100;
kazuryu 0:3dc012104243 80 int c = int((a-b*100)/10);
kazuryu 0:3dc012104243 81 int d = int(a-b*100-c*10);
kazuryu 0:3dc012104243 82 st[0] = ('0'+b);
kazuryu 0:3dc012104243 83 st[1] = ('0'+c);
kazuryu 0:3dc012104243 84 st[2] = ('0'+d);
kazuryu 0:3dc012104243 85 return(st);
kazuryu 0:3dc012104243 86 }else if(a > 9){
kazuryu 0:3dc012104243 87 char st[2];
kazuryu 0:3dc012104243 88 int b = (int)a/10;
kazuryu 0:3dc012104243 89 int c = int(a-b*10);
kazuryu 0:3dc012104243 90 st[0] ='0'+b;
kazuryu 0:3dc012104243 91 st[1] ='0'+c;
kazuryu 0:3dc012104243 92 return(st);
kazuryu 0:3dc012104243 93 }else{
kazuryu 0:3dc012104243 94 char st[1];
kazuryu 0:3dc012104243 95 st[0] = '0'+a;
kazuryu 0:3dc012104243 96 return(st);
kazuryu 0:3dc012104243 97 }
kazuryu 0:3dc012104243 98 }
kazuryu 0:3dc012104243 99 void i2c_write(int addr,char regist,char data)
kazuryu 0:3dc012104243 100 {
kazuryu 0:3dc012104243 101 funcd[0]=regist;
kazuryu 0:3dc012104243 102 funcd[1]=data;
kazuryu 0:3dc012104243 103 i2c.write(addr,funcd,2);
kazuryu 0:3dc012104243 104 }
kazuryu 0:3dc012104243 105
kazuryu 0:3dc012104243 106 char i2c_read(int addr,char regist)
kazuryu 0:3dc012104243 107 {
kazuryu 0:3dc012104243 108 funcd[0]=regist;
kazuryu 0:3dc012104243 109 i2c.write(addr,funcd,1);
kazuryu 0:3dc012104243 110 i2c.read(addr,funcd,1);
kazuryu 0:3dc012104243 111 return funcd[0];
kazuryu 0:3dc012104243 112 }
kazuryu 0:3dc012104243 113
kazuryu 0:3dc012104243 114 int i2c_read_16bit(int addr,char registL,char registH)
kazuryu 0:3dc012104243 115 {
kazuryu 0:3dc012104243 116 return (int(i2c_read(addr,registH))<<8)+int(i2c_read(addr,registL));
kazuryu 0:3dc012104243 117 }
kazuryu 0:3dc012104243 118
kazuryu 0:3dc012104243 119 void offset_adjust(double* all,double* gz){
kazuryu 0:3dc012104243 120 if(!(*gz-offset_gz < 1 && *gz-offset_gz > -1)){
kazuryu 0:3dc012104243 121 *all += (*gz-offset_gz)*elapsed_time*0.001;
kazuryu 0:3dc012104243 122 }
kazuryu 0:3dc012104243 123 }
kazuryu 0:3dc012104243 124
kazuryu 0:3dc012104243 125 float angle_adjust(float Gein,double all,float Min_ang,float Min_pwm,float Max_pwm_roll){
kazuryu 0:3dc012104243 126 float pwm;
kazuryu 0:3dc012104243 127 if(all > 0){
kazuryu 0:3dc012104243 128 pwm = all*Gein;
kazuryu 0:3dc012104243 129 }else{
kazuryu 0:3dc012104243 130 pwm = -all*Gein;
kazuryu 0:3dc012104243 131 }
kazuryu 0:3dc012104243 132 if(all < Min_ang && all > -Min_ang){
kazuryu 0:3dc012104243 133 pwm = 0;
kazuryu 0:3dc012104243 134 }
kazuryu 0:3dc012104243 135 if(pwm < Min_pwm)pwm = 0;
kazuryu 0:3dc012104243 136 if(pwm > Max_pwm_roll){
kazuryu 0:3dc012104243 137 pwm = Max_pwm_roll;
kazuryu 0:3dc012104243 138 }
kazuryu 0:3dc012104243 139 return(pwm);
kazuryu 0:3dc012104243 140 }