Ryu Kaz / Mbed OS 767zi

t6est

Dependencies:   Pulse

functions.cpp@4:9ba47e5db1e2, 2019-10-05 (annotated)

Committer:
kazuryu
Date:
Sat Oct 05 01:41:37 2019 +0000
Revision:
4:9ba47e5db1e2
Parent:
0:3dc012104243 
B

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 4:9ba47e5db1e2 5 const int Sampling = 3000;
kazuryu 0:3dc012104243 6
kazuryu 0:3dc012104243 7 I2C i2c(PB_11 , PB_10);//PB_11 -> PB_9
kazuryu 4:9ba47e5db1e2 8 // -> 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 4:9ba47e5db1e2 36 offset_gz = 0;
kazuryu 0:3dc012104243 37 for(int i =0;i<Sampling;i++){
kazuryu 0:3dc012104243 38
kazuryu 0:3dc012104243 39 double yaw;
kazuryu 0:3dc012104243 40 readGyz(&yaw);
kazuryu 0:3dc012104243 41 if(yaw == 139.323480)error_c++;
kazuryu 0:3dc012104243 42 //ss.printf("%f\n",yaw);
kazuryu 0:3dc012104243 43 offset_gz += yaw;
kazuryu 0:3dc012104243 44 }
kazuryu 0:3dc012104243 45 offset_gz /= Sampling;
kazuryu 0:3dc012104243 46 }
kazuryu 0:3dc012104243 47
kazuryu 0:3dc012104243 48 int rez(){
kazuryu 0:3dc012104243 49 return(error_c);
kazuryu 0:3dc012104243 50 }
kazuryu 0:3dc012104243 51
kazuryu 0:3dc012104243 52 void to_signed(double* all){
kazuryu 0:3dc012104243 53 if(*all < 0){
kazuryu 0:3dc012104243 54 *all += 360;
kazuryu 0:3dc012104243 55 }
kazuryu 0:3dc012104243 56 if(*all >= 360){
kazuryu 0:3dc012104243 57 *all -= 360;
kazuryu 0:3dc012104243 58 }
kazuryu 0:3dc012104243 59
kazuryu 0:3dc012104243 60 if(*all >= 180){
kazuryu 0:3dc012104243 61 *all -= 360;
kazuryu 0:3dc012104243 62 }
kazuryu 0:3dc012104243 63 }
kazuryu 0:3dc012104243 64
kazuryu 0:3dc012104243 65 char *puc(double all){
kazuryu 0:3dc012104243 66 int a = int(all);
kazuryu 0:3dc012104243 67 if(a > 999){
kazuryu 0:3dc012104243 68 char st[4];
kazuryu 0:3dc012104243 69 int b = (int)a/1000;
kazuryu 0:3dc012104243 70 int c = int((a-b*1000)/100);
kazuryu 0:3dc012104243 71 int d = int((a-b*1000-c*100)/10);
kazuryu 0:3dc012104243 72 int e = a-b*1000-c*100-d*10;
kazuryu 0:3dc012104243 73 st[0] = ('0'+b);
kazuryu 0:3dc012104243 74 st[1] = ('0'+c);
kazuryu 0:3dc012104243 75 st[2] = ('0'+d);
kazuryu 0:3dc012104243 76 st[3] = ('0'+e);
kazuryu 0:3dc012104243 77 return(st);
kazuryu 0:3dc012104243 78 }else if(a > 99){//3桁
kazuryu 0:3dc012104243 79 char st[3];
kazuryu 0:3dc012104243 80 int b = (int)a/100;
kazuryu 0:3dc012104243 81 int c = int((a-b*100)/10);
kazuryu 0:3dc012104243 82 int d = int(a-b*100-c*10);
kazuryu 0:3dc012104243 83 st[0] = ('0'+b);
kazuryu 0:3dc012104243 84 st[1] = ('0'+c);
kazuryu 0:3dc012104243 85 st[2] = ('0'+d);
kazuryu 0:3dc012104243 86 return(st);
kazuryu 0:3dc012104243 87 }else if(a > 9){
kazuryu 0:3dc012104243 88 char st[2];
kazuryu 0:3dc012104243 89 int b = (int)a/10;
kazuryu 0:3dc012104243 90 int c = int(a-b*10);
kazuryu 0:3dc012104243 91 st[0] ='0'+b;
kazuryu 0:3dc012104243 92 st[1] ='0'+c;
kazuryu 0:3dc012104243 93 return(st);
kazuryu 0:3dc012104243 94 }else{
kazuryu 0:3dc012104243 95 char st[1];
kazuryu 0:3dc012104243 96 st[0] = '0'+a;
kazuryu 0:3dc012104243 97 return(st);
kazuryu 0:3dc012104243 98 }
kazuryu 0:3dc012104243 99 }
kazuryu 0:3dc012104243 100 void i2c_write(int addr,char regist,char data)
kazuryu 0:3dc012104243 101 {
kazuryu 0:3dc012104243 102 funcd[0]=regist;
kazuryu 0:3dc012104243 103 funcd[1]=data;
kazuryu 0:3dc012104243 104 i2c.write(addr,funcd,2);
kazuryu 0:3dc012104243 105 }
kazuryu 0:3dc012104243 106
kazuryu 0:3dc012104243 107 char i2c_read(int addr,char regist)
kazuryu 0:3dc012104243 108 {
kazuryu 0:3dc012104243 109 funcd[0]=regist;
kazuryu 0:3dc012104243 110 i2c.write(addr,funcd,1);
kazuryu 0:3dc012104243 111 i2c.read(addr,funcd,1);
kazuryu 0:3dc012104243 112 return funcd[0];
kazuryu 0:3dc012104243 113 }
kazuryu 0:3dc012104243 114
kazuryu 0:3dc012104243 115 int i2c_read_16bit(int addr,char registL,char registH)
kazuryu 0:3dc012104243 116 {
kazuryu 0:3dc012104243 117 return (int(i2c_read(addr,registH))<<8)+int(i2c_read(addr,registL));
kazuryu 0:3dc012104243 118 }
kazuryu 0:3dc012104243 119
kazuryu 0:3dc012104243 120 void offset_adjust(double* all,double* gz){
kazuryu 0:3dc012104243 121 if(!(*gz-offset_gz < 1 && *gz-offset_gz > -1)){
kazuryu 0:3dc012104243 122 *all += (*gz-offset_gz)*elapsed_time*0.001;
kazuryu 0:3dc012104243 123 }
kazuryu 0:3dc012104243 124 }
kazuryu 0:3dc012104243 125
kazuryu 0:3dc012104243 126 float angle_adjust(float Gein,double all,float Min_ang,float Min_pwm,float Max_pwm_roll){
kazuryu 0:3dc012104243 127 float pwm;
kazuryu 0:3dc012104243 128 if(all > 0){
kazuryu 0:3dc012104243 129 pwm = all*Gein;
kazuryu 0:3dc012104243 130 }else{
kazuryu 0:3dc012104243 131 pwm = -all*Gein;
kazuryu 0:3dc012104243 132 }
kazuryu 0:3dc012104243 133 if(all < Min_ang && all > -Min_ang){
kazuryu 0:3dc012104243 134 pwm = 0;
kazuryu 0:3dc012104243 135 }
kazuryu 0:3dc012104243 136 if(pwm < Min_pwm)pwm = 0;
kazuryu 0:3dc012104243 137 if(pwm > Max_pwm_roll){
kazuryu 0:3dc012104243 138 pwm = Max_pwm_roll;
kazuryu 0:3dc012104243 139 }
kazuryu 0:3dc012104243 140 return(pwm);
kazuryu 0:3dc012104243 141 }