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@2:4a512a6e6b1d, 2021-05-13 (annotated)

Committer:
sato_shi
Date:
Thu May 13 05:12:39 2021 +0000
Revision:
2:4a512a6e6b1d
Parent:
0:d6107da1cba0
Child:
3:94b960cbb1bd
BBBBBBBB

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 2:4a512a6e6b1d 17 int t_1, t_2;
sato_shi 2:4a512a6e6b1d 18 int i = -1;
sato_shi 2:4a512a6e6b1d 19 int j;
sato_shi 2:4a512a6e6b1d 20 bool flug = false;
sato_shi 2:4a512a6e6b1d 21 unsigned long tm, tm2, tm3 = 0;
sato_shi 2:4a512a6e6b1d 22 int ax,ay,az;
sato_shi 2:4a512a6e6b1d 23 //float ax2,ay2,az2;
sato_shi 2:4a512a6e6b1d 24 float Role = 0;
sato_shi 2:4a512a6e6b1d 25 int num;
sato_shi 2:4a512a6e6b1d 26 char ch;
sato_shi 2:4a512a6e6b1d 27 float w = 0;
sato_shi 2:4a512a6e6b1d 28 float dw = 0.1;
sato_shi 2:4a512a6e6b1d 29
sato_shi 2:4a512a6e6b1d 30 //初期設定
sato_shi 2:4a512a6e6b1d 31 //加速度センサ
sato_shi 2:4a512a6e6b1d 32 //MPU6050 mpu1(p9,p10,0x68);
sato_shi 2:4a512a6e6b1d 33 //MPU6050 mpu2(p9,p10,0x69);
sato_shi 2:4a512a6e6b1d 34 //シリアル
Satoshisatoshi 0:d6107da1cba0 35 Serial pc(USBTX,USBRX);
sato_shi 2:4a512a6e6b1d 36 //エンコーダ
sato_shi 2:4a512a6e6b1d 37 QEI wheel(p22, p21, NC, 6, QEI::X2_ENCODING);
sato_shi 2:4a512a6e6b1d 38 //タイマー
sato_shi 2:4a512a6e6b1d 39 Ticker flipper;
sato_shi 2:4a512a6e6b1d 40 void flip() {
sato_shi 2:4a512a6e6b1d 41 flug = true;
sato_shi 2:4a512a6e6b1d 42 //pc.printf("AA");
sato_shi 2:4a512a6e6b1d 43 }
sato_shi 2:4a512a6e6b1d 44 Timer t;
sato_shi 2:4a512a6e6b1d 45 //モータ
Satoshisatoshi 0:d6107da1cba0 46
Satoshisatoshi 0:d6107da1cba0 47
Satoshisatoshi 0:d6107da1cba0 48 int main(){
sato_shi 2:4a512a6e6b1d 49 TB6612FNG motor(p18, p19, p20, p26);
sato_shi 2:4a512a6e6b1d 50 ax = 0;
sato_shi 2:4a512a6e6b1d 51 ay = 1;
sato_shi 2:4a512a6e6b1d 52 az = 3;
sato_shi 2:4a512a6e6b1d 53 Serial pc(USBTX,USBRX);
sato_shi 2:4a512a6e6b1d 54 pc.baud(115200);
sato_shi 2:4a512a6e6b1d 55 //pc.baud(9600);
sato_shi 2:4a512a6e6b1d 56 //pc.baud(230400);
sato_shi 2:4a512a6e6b1d 57 //pc.baud(200000);
sato_shi 2:4a512a6e6b1d 58 //mpu1.start();
sato_shi 2:4a512a6e6b1d 59 //mpu2.start();
sato_shi 2:4a512a6e6b1d 60 //mpu1.start();
sato_shi 2:4a512a6e6b1d 61 //mpu2.start();
sato_shi 2:4a512a6e6b1d 62 motor.STBY(1);
sato_shi 2:4a512a6e6b1d 63 motor.Forward();
sato_shi 2:4a512a6e6b1d 64 //pc.printf("Hello3\n");
Satoshisatoshi 0:d6107da1cba0 65 while(1){
sato_shi 2:4a512a6e6b1d 66 if(i == -1){
sato_shi 2:4a512a6e6b1d 67 if(pc.readable()) {
sato_shi 2:4a512a6e6b1d 68 char ch[5]; // 受信確認
sato_shi 2:4a512a6e6b1d 69 while(pc.readable()){
sato_shi 2:4a512a6e6b1d 70 if(j == 0)wait(0.1);
sato_shi 2:4a512a6e6b1d 71 //pc.printf("%d\n", j);
sato_shi 2:4a512a6e6b1d 72 ch[j] = pc.getc();
sato_shi 2:4a512a6e6b1d 73 j++;
sato_shi 2:4a512a6e6b1d 74 //wait(0.01);
sato_shi 2:4a512a6e6b1d 75 } // 1文字取り出し
sato_shi 2:4a512a6e6b1d 76 ch[j] = '\0';
sato_shi 2:4a512a6e6b1d 77 num = atoi(ch);
sato_shi 2:4a512a6e6b1d 78 j = 0;
sato_shi 2:4a512a6e6b1d 79 switch(num){
sato_shi 2:4a512a6e6b1d 80 case -1:
sato_shi 2:4a512a6e6b1d 81 i = 0;
sato_shi 2:4a512a6e6b1d 82 flipper.attach_us(&flip, period);
sato_shi 2:4a512a6e6b1d 83 break;
sato_shi 2:4a512a6e6b1d 84 case -2:
sato_shi 2:4a512a6e6b1d 85 motor.Stop();
sato_shi 2:4a512a6e6b1d 86 break;
sato_shi 2:4a512a6e6b1d 87 case -3:
sato_shi 2:4a512a6e6b1d 88 motor.Forward();
sato_shi 2:4a512a6e6b1d 89 break;
sato_shi 2:4a512a6e6b1d 90 case -4:
sato_shi 2:4a512a6e6b1d 91 break;
sato_shi 2:4a512a6e6b1d 92 default:
sato_shi 2:4a512a6e6b1d 93 motor.SetW(float(num)/255);
sato_shi 2:4a512a6e6b1d 94 break;
sato_shi 2:4a512a6e6b1d 95 }
sato_shi 2:4a512a6e6b1d 96 }
sato_shi 2:4a512a6e6b1d 97 }
sato_shi 2:4a512a6e6b1d 98 // t.reset();
sato_shi 2:4a512a6e6b1d 99 // t.start();
sato_shi 2:4a512a6e6b1d 100 if(flug){
sato_shi 2:4a512a6e6b1d 101 //tm = t.read_us();
sato_shi 2:4a512a6e6b1d 102 //tm2 = tm - tm3;
sato_shi 2:4a512a6e6b1d 103 //tm3 = tm;
sato_shi 2:4a512a6e6b1d 104 flug = false;
sato_shi 2:4a512a6e6b1d 105 if(!i){
sato_shi 2:4a512a6e6b1d 106 t_2 = 0;
sato_shi 2:4a512a6e6b1d 107 t.reset();
sato_shi 2:4a512a6e6b1d 108 wheel.reset();
sato_shi 2:4a512a6e6b1d 109 t.start();
sato_shi 2:4a512a6e6b1d 110 }
sato_shi 2:4a512a6e6b1d 111 //pc.printf("Hello\n");
sato_shi 2:4a512a6e6b1d 112 //mpu1.read(&gx2,&gy2,&gz2,&ax2,&ay2,&az2);
sato_shi 2:4a512a6e6b1d 113 //pc.printf("%f\n%f\n%f\n",ax2,ay2,az2);
sato_shi 2:4a512a6e6b1d 114 //mpu1.readraw2(&ax,&ay,&az);
sato_shi 2:4a512a6e6b1d 115 pc.printf("%d\n%d\n%d\n",ax,ay,az);
sato_shi 2:4a512a6e6b1d 116 //mpu2.read(&gx2,&gy2,&gz2,&ax2,&ay2,&az2);
sato_shi 2:4a512a6e6b1d 117 //pc.printf("%f\n%f\n%f\n",ax2,ay2,az2);
sato_shi 2:4a512a6e6b1d 118 //mpu2.readraw2(&ax,&ay,&az);
sato_shi 2:4a512a6e6b1d 119 pc.printf("%d\n%d\n%d\n",ax,ay,az);
sato_shi 2:4a512a6e6b1d 120 t_1 = t.read_us();
sato_shi 2:4a512a6e6b1d 121 pc.printf("%d\n", t_1 - t_2);
sato_shi 2:4a512a6e6b1d 122 t_2 = t_1;
sato_shi 2:4a512a6e6b1d 123 i++;
sato_shi 2:4a512a6e6b1d 124 if(i >= rate){
sato_shi 2:4a512a6e6b1d 125 flipper.detach();
sato_shi 2:4a512a6e6b1d 126 t_1 = t.read_us();
sato_shi 2:4a512a6e6b1d 127 Role = ((float)wheel.getPulses())*1000000 / (6*t_1);
sato_shi 2:4a512a6e6b1d 128 pc.printf("%f\n", Role);
sato_shi 2:4a512a6e6b1d 129 i = -1;
sato_shi 2:4a512a6e6b1d 130 }
sato_shi 2:4a512a6e6b1d 131 }
Satoshisatoshi 0:d6107da1cba0 132 }
sato_shi 2:4a512a6e6b1d 133 }