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@3:94b960cbb1bd, 2021-05-14 (annotated)

Committer:
sato_shi
Date:
Fri May 14 00:48:28 2021 +0000
Revision:
3:94b960cbb1bd
Parent:
2:4a512a6e6b1d
Child:
4:341ad97bf8a6
CCC;

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 2:4a512a6e6b1d 15 #define period 100000
sato_shi 2:4a512a6e6b1d 16 #define rate 32
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 3:94b960cbb1bd 52 if (mpu1.read_data(mpu1.ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6)) {
sato_shi 3:94b960cbb1bd 53 pc.printf("%d\n", int(data2[0] << 8 | data2[1]));
sato_shi 3:94b960cbb1bd 54 t_11 = t.read_us();
sato_shi 3:94b960cbb1bd 55 pc.printf("%d\n", t_11 - t_2);
sato_shi 3:94b960cbb1bd 56 pc.printf("%d\n", int(data2[2] << 8 | data2[3]));
sato_shi 3:94b960cbb1bd 57 t_12 = t.read_us();
sato_shi 3:94b960cbb1bd 58 pc.printf("%d\n", t_12 - t_2);
sato_shi 3:94b960cbb1bd 59 pc.printf("%d\n", int(data2[4] << 8 | data2[5]));
sato_shi 3:94b960cbb1bd 60 }
sato_shi 3:94b960cbb1bd 61 //pc.printf("0\n1\n2\n");
sato_shi 3:94b960cbb1bd 62 t_3 = t.read_us();
sato_shi 3:94b960cbb1bd 63 pc.printf("%d\n", t_3 - t_2);
sato_shi 3:94b960cbb1bd 64 if (mpu2.read_data(mpu2.ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6)) {
sato_shi 3:94b960cbb1bd 65 pc.printf("%d\n", int(data2[0] << 8 | data2[1]));
sato_shi 3:94b960cbb1bd 66 t_21 = t.read_us();
sato_shi 3:94b960cbb1bd 67 pc.printf("%d\n", t_21 - t_2);
sato_shi 3:94b960cbb1bd 68 pc.printf("%d\n", int(data2[2] << 8 | data2[3]));
sato_shi 3:94b960cbb1bd 69 t_22 = t.read_us();
sato_shi 3:94b960cbb1bd 70 pc.printf("%d\n", t_22 - t_2);
sato_shi 3:94b960cbb1bd 71 pc.printf("%d\n", int(data2[4] << 8 | data2[5]));
sato_shi 3:94b960cbb1bd 72 }
sato_shi 3:94b960cbb1bd 73 //pc.printf("%d\n%d\n%d\n",ax,ay,az);
sato_shi 3:94b960cbb1bd 74 //pc.printf("0\n1\n2\n");
sato_shi 3:94b960cbb1bd 75 t_4 = t.read_us();
sato_shi 3:94b960cbb1bd 76 pc.printf("%d\n", t_4 - t_2);
sato_shi 3:94b960cbb1bd 77 t_2 = t_1;
sato_shi 3:94b960cbb1bd 78 i--;
sato_shi 3:94b960cbb1bd 79 if(!i){
sato_shi 3:94b960cbb1bd 80 flipper.detach();
sato_shi 3:94b960cbb1bd 81 t_1 = t.read_us();
sato_shi 3:94b960cbb1bd 82 Role = ((float)wheel.getPulses())*1000000 / (6*t_1);
sato_shi 3:94b960cbb1bd 83 pc.printf("%f\n", Role);
sato_shi 3:94b960cbb1bd 84 flug = true;
sato_shi 3:94b960cbb1bd 85 i = rate;
sato_shi 3:94b960cbb1bd 86 }
sato_shi 2:4a512a6e6b1d 87 }
Satoshisatoshi 0:d6107da1cba0 88
Satoshisatoshi 0:d6107da1cba0 89
Satoshisatoshi 0:d6107da1cba0 90 int main(){
sato_shi 2:4a512a6e6b1d 91 TB6612FNG motor(p18, p19, p20, p26);
sato_shi 3:94b960cbb1bd 92 //pc.baud(115200);
sato_shi 3:94b960cbb1bd 93 //pc.baud(9600);
sato_shi 3:94b960cbb1bd 94 pc.baud(230400);
sato_shi 3:94b960cbb1bd 95 //pc.baud(200000);
sato_shi 3:94b960cbb1bd 96
sato_shi 3:94b960cbb1bd 97 mpu1.start();
sato_shi 3:94b960cbb1bd 98 mpu2.start();
sato_shi 2:4a512a6e6b1d 99 //mpu1.start();
sato_shi 2:4a512a6e6b1d 100 //mpu2.start();
sato_shi 2:4a512a6e6b1d 101 motor.STBY(1);
sato_shi 2:4a512a6e6b1d 102 motor.Forward();
sato_shi 2:4a512a6e6b1d 103 //pc.printf("Hello3\n");
Satoshisatoshi 0:d6107da1cba0 104 while(1){
sato_shi 3:94b960cbb1bd 105 if(flug){
sato_shi 2:4a512a6e6b1d 106 if(pc.readable()) {
sato_shi 2:4a512a6e6b1d 107 char ch[5]; // 受信確認
sato_shi 2:4a512a6e6b1d 108 while(pc.readable()){
sato_shi 2:4a512a6e6b1d 109 if(j == 0)wait(0.1);
sato_shi 2:4a512a6e6b1d 110 //pc.printf("%d\n", j);
sato_shi 2:4a512a6e6b1d 111 ch[j] = pc.getc();
sato_shi 2:4a512a6e6b1d 112 j++;
sato_shi 2:4a512a6e6b1d 113 //wait(0.01);
sato_shi 2:4a512a6e6b1d 114 } // 1文字取り出し
sato_shi 2:4a512a6e6b1d 115 ch[j] = '\0';
sato_shi 2:4a512a6e6b1d 116 num = atoi(ch);
sato_shi 2:4a512a6e6b1d 117 j = 0;
sato_shi 2:4a512a6e6b1d 118 switch(num){
sato_shi 2:4a512a6e6b1d 119 case -1:
sato_shi 3:94b960cbb1bd 120 flug = false;
sato_shi 3:94b960cbb1bd 121 t_2 = 0;
sato_shi 3:94b960cbb1bd 122 t.reset();
sato_shi 3:94b960cbb1bd 123 wheel.reset();
sato_shi 3:94b960cbb1bd 124 t.start();
sato_shi 3:94b960cbb1bd 125 flipper.attach(&flip, 0.1);
sato_shi 2:4a512a6e6b1d 126 break;
sato_shi 2:4a512a6e6b1d 127 case -2:
sato_shi 2:4a512a6e6b1d 128 motor.Stop();
sato_shi 2:4a512a6e6b1d 129 break;
sato_shi 2:4a512a6e6b1d 130 case -3:
sato_shi 2:4a512a6e6b1d 131 motor.Forward();
sato_shi 2:4a512a6e6b1d 132 break;
sato_shi 2:4a512a6e6b1d 133 case -4:
sato_shi 2:4a512a6e6b1d 134 break;
sato_shi 2:4a512a6e6b1d 135 default:
sato_shi 2:4a512a6e6b1d 136 motor.SetW(float(num)/255);
sato_shi 2:4a512a6e6b1d 137 break;
sato_shi 2:4a512a6e6b1d 138 }
sato_shi 2:4a512a6e6b1d 139 }
sato_shi 2:4a512a6e6b1d 140 }
Satoshisatoshi 0:d6107da1cba0 141 }
sato_shi 2:4a512a6e6b1d 142 }