Satoshi Iyobe / Mbed 2 deprecated measure_1_DDD

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 QEI MPU6050 TB6612FNG

main.cpp@4:341ad97bf8a6, 2021-05-14 (annotated)

Committer:
sato_shi
Date:
Fri May 14 01:29:56 2021 +0000
Revision:
4:341ad97bf8a6
Parent:
3:94b960cbb1bd 
EEE

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Satoshisatoshi 0:d6107da1cba0 1 #include "mbed.h"
sato_shi 2:4a512a6e6b1d 2 #include <stdlib.h>
Satoshisatoshi 0:d6107da1cba0 3 #include "MPU6050.h"
Satoshisatoshi 0:d6107da1cba0 4 #include "QEI.h"
Satoshisatoshi 0:d6107da1cba0 5 #include "TB6612FNG.h"
Satoshisatoshi 0:d6107da1cba0 6
sato_shi 2:4a512a6e6b1d 7 #define MPU6050_WHO_AM_I 0x75 // Read Only
sato_shi 2:4a512a6e6b1d 8 #define MPU6050_PWR_MGMT_1 0x6B // Read and Write
sato_shi 2:4a512a6e6b1d 9 #define MPU_ADDRESS 0x68
sato_shi 2:4a512a6e6b1d 10 #define MPU_ADDRESS2 0x69
sato_shi 2:4a512a6e6b1d 11 #define TYP1 16384.0 //
sato_shi 2:4a512a6e6b1d 12 #define TYP2 8192.0 //
sato_shi 2:4a512a6e6b1d 13 #define TYP3 4096.0 //
sato_shi 2:4a512a6e6b1d 14 #define TYP4 2048.0 //
sato_shi 4:341ad97bf8a6 15 #define period 4000
sato_shi 4:341ad97bf8a6 16 #define rate 128
sato_shi 3:94b960cbb1bd 17 int t_1, t_2, t_3, t_4;
sato_shi 3:94b960cbb1bd 18 int t_21, t_22, t_23;
sato_shi 3:94b960cbb1bd 19 int t_11, t_12, t_13;
sato_shi 3:94b960cbb1bd 20 int i = rate;
sato_shi 2:4a512a6e6b1d 21 int j;
sato_shi 3:94b960cbb1bd 22 bool flug = true;
sato_shi 2:4a512a6e6b1d 23 unsigned long tm, tm2, tm3 = 0;
sato_shi 3:94b960cbb1bd 24 int16_t ax,ay,az;
sato_shi 2:4a512a6e6b1d 25 //float ax2,ay2,az2;
sato_shi 2:4a512a6e6b1d 26 float Role = 0;
sato_shi 2:4a512a6e6b1d 27 int num;
sato_shi 2:4a512a6e6b1d 28 char ch;
sato_shi 2:4a512a6e6b1d 29 float w = 0;
sato_shi 2:4a512a6e6b1d 30 float dw = 0.1;
sato_shi 3:94b960cbb1bd 31 Timer t;
sato_shi 2:4a512a6e6b1d 32 //初期設定
sato_shi 2:4a512a6e6b1d 33 //加速度センサ
sato_shi 3:94b960cbb1bd 34 MPU6050 mpu1(p9,p10,0x68);
sato_shi 3:94b960cbb1bd 35 MPU6050 mpu2(p9,p10,0x69);
sato_shi 3:94b960cbb1bd 36 Serial pc(USBTX,USBRX);
sato_shi 3:94b960cbb1bd 37 //pc.baud(115200);
sato_shi 3:94b960cbb1bd 38 //pc.baud(9600);
sato_shi 3:94b960cbb1bd 39 //pc.baud(230400);
sato_shi 3:94b960cbb1bd 40 //pc.baud(200000);
sato_shi 3:94b960cbb1bd 41 char data2[6];
sato_shi 3:94b960cbb1bd 42 char data3[6];
sato_shi 2:4a512a6e6b1d 43 //シリアル
sato_shi 3:94b960cbb1bd 44 //Serial pc(USBTX,USBRX);
sato_shi 2:4a512a6e6b1d 45 //エンコーダ
sato_shi 2:4a512a6e6b1d 46 QEI wheel(p22, p21, NC, 6, QEI::X2_ENCODING);
sato_shi 2:4a512a6e6b1d 47 //タイマー
sato_shi 2:4a512a6e6b1d 48 Ticker flipper;
sato_shi 2:4a512a6e6b1d 49 void flip() {
sato_shi 3:94b960cbb1bd 50 t_1 = t.read_us();
sato_shi 3:94b960cbb1bd 51 pc.printf("%d\n", t_1 - t_2);
sato_shi 4:341ad97bf8a6 52 if (mpu1.read_data(mpu1.ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6)){
sato_shi 4:341ad97bf8a6 53 t_3 = t.read_us();
sato_shi 4:341ad97bf8a6 54 pc.printf("%d\n", t_3 - t_2);
sato_shi 4:341ad97bf8a6 55 if (mpu2.read_data(mpu2.ADDRESS, MPU6050_ACCEL_XOUT_H, data3, 6)) {
sato_shi 4:341ad97bf8a6 56 t_4 = t.read_us();
sato_shi 4:341ad97bf8a6 57 pc.printf("%d\n", t_4 - t_2);
sato_shi 4:341ad97bf8a6 58 pc.printf("%d\n", int(data2[0] << 8 | data2[1]));
sato_shi 4:341ad97bf8a6 59 pc.printf("%d\n", int(data2[2] << 8 | data2[3]));
sato_shi 4:341ad97bf8a6 60 pc.printf("%d\n", int(data2[4] << 8 | data2[5]));
sato_shi 4:341ad97bf8a6 61 pc.printf("%d\n", int(data3[0] << 8 | data3[1]));
sato_shi 4:341ad97bf8a6 62 pc.printf("%d\n", int(data3[2] << 8 | data3[3]));
sato_shi 4:341ad97bf8a6 63 pc.printf("%d\n", int(data3[4] << 8 | data3[5]));
sato_shi 4:341ad97bf8a6 64 t_2 = t_1;
sato_shi 4:341ad97bf8a6 65 i--;
sato_shi 4:341ad97bf8a6 66 }
sato_shi 3:94b960cbb1bd 67 }
sato_shi 3:94b960cbb1bd 68 if(!i){
sato_shi 3:94b960cbb1bd 69 flipper.detach();
sato_shi 3:94b960cbb1bd 70 t_1 = t.read_us();
sato_shi 3:94b960cbb1bd 71 Role = ((float)wheel.getPulses())*1000000 / (6*t_1);
sato_shi 3:94b960cbb1bd 72 pc.printf("%f\n", Role);
sato_shi 3:94b960cbb1bd 73 flug = true;
sato_shi 3:94b960cbb1bd 74 i = rate;
sato_shi 3:94b960cbb1bd 75 }
sato_shi 2:4a512a6e6b1d 76 }
Satoshisatoshi 0:d6107da1cba0 77
Satoshisatoshi 0:d6107da1cba0 78
Satoshisatoshi 0:d6107da1cba0 79 int main(){
sato_shi 2:4a512a6e6b1d 80 TB6612FNG motor(p18, p19, p20, p26);
sato_shi 3:94b960cbb1bd 81 //pc.baud(115200);
sato_shi 3:94b960cbb1bd 82 //pc.baud(9600);
sato_shi 3:94b960cbb1bd 83 pc.baud(230400);
sato_shi 3:94b960cbb1bd 84 //pc.baud(200000);
sato_shi 3:94b960cbb1bd 85
sato_shi 3:94b960cbb1bd 86 mpu1.start();
sato_shi 3:94b960cbb1bd 87 mpu2.start();
sato_shi 2:4a512a6e6b1d 88 //mpu1.start();
sato_shi 2:4a512a6e6b1d 89 //mpu2.start();
sato_shi 2:4a512a6e6b1d 90 motor.STBY(1);
sato_shi 2:4a512a6e6b1d 91 motor.Forward();
sato_shi 2:4a512a6e6b1d 92 //pc.printf("Hello3\n");
Satoshisatoshi 0:d6107da1cba0 93 while(1){
sato_shi 3:94b960cbb1bd 94 if(flug){
sato_shi 2:4a512a6e6b1d 95 if(pc.readable()) {
sato_shi 2:4a512a6e6b1d 96 char ch[5]; // 受信確認
sato_shi 2:4a512a6e6b1d 97 while(pc.readable()){
sato_shi 2:4a512a6e6b1d 98 if(j == 0)wait(0.1);
sato_shi 2:4a512a6e6b1d 99 //pc.printf("%d\n", j);
sato_shi 2:4a512a6e6b1d 100 ch[j] = pc.getc();
sato_shi 2:4a512a6e6b1d 101 j++;
sato_shi 2:4a512a6e6b1d 102 //wait(0.01);
sato_shi 2:4a512a6e6b1d 103 } // 1文字取り出し
sato_shi 2:4a512a6e6b1d 104 ch[j] = '\0';
sato_shi 2:4a512a6e6b1d 105 num = atoi(ch);
sato_shi 2:4a512a6e6b1d 106 j = 0;
sato_shi 2:4a512a6e6b1d 107 switch(num){
sato_shi 2:4a512a6e6b1d 108 case -1:
sato_shi 3:94b960cbb1bd 109 flug = false;
sato_shi 3:94b960cbb1bd 110 t_2 = 0;
sato_shi 3:94b960cbb1bd 111 t.reset();
sato_shi 3:94b960cbb1bd 112 wheel.reset();
sato_shi 3:94b960cbb1bd 113 t.start();
sato_shi 3:94b960cbb1bd 114 flipper.attach(&flip, 0.1);
sato_shi 2:4a512a6e6b1d 115 break;
sato_shi 2:4a512a6e6b1d 116 case -2:
sato_shi 2:4a512a6e6b1d 117 motor.Stop();
sato_shi 2:4a512a6e6b1d 118 break;
sato_shi 2:4a512a6e6b1d 119 case -3:
sato_shi 2:4a512a6e6b1d 120 motor.Forward();
sato_shi 2:4a512a6e6b1d 121 break;
sato_shi 2:4a512a6e6b1d 122 case -4:
sato_shi 2:4a512a6e6b1d 123 break;
sato_shi 2:4a512a6e6b1d 124 default:
sato_shi 2:4a512a6e6b1d 125 motor.SetW(float(num)/255);
sato_shi 2:4a512a6e6b1d 126 break;
sato_shi 2:4a512a6e6b1d 127 }
sato_shi 2:4a512a6e6b1d 128 }
sato_shi 2:4a512a6e6b1d 129 }
Satoshisatoshi 0:d6107da1cba0 130 }
sato_shi 2:4a512a6e6b1d 131 }