MMlab / Mbed 2 deprecated PING_ultrasonic

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:   USBDevice mbed

main.cpp@4:00ac3d093b24, 2016-09-26 (annotated)

Committer:
ikuta
Date:
Mon Sep 26 14:01:47 2016 +0000
Revision:
4:00ac3d093b24
Parent:
3:814cb36d0f90
Child:
5:62de7a36d3af
????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ikuta 0:5d1796a0f9ce 1 #include "mbed.h"
ikuta 1:5aa590d1f1bf 2 #include "stdio.h"
ikuta 1:5aa590d1f1bf 3 #include "math.h"
ikuta 4:00ac3d093b24 4 #include "USBJoystick.h"
ikuta 0:5d1796a0f9ce 5
ikuta 0:5d1796a0f9ce 6 Serial pc(USBTX,USBRX);
ikuta 4:00ac3d093b24 7 USBJoystick joystick;
ikuta 0:5d1796a0f9ce 8
ikuta 0:5d1796a0f9ce 9 DigitalInOut pingPin1(p18);
ikuta 0:5d1796a0f9ce 10 DigitalInOut pingPin2(p19);
ikuta 0:5d1796a0f9ce 11
ikuta 0:5d1796a0f9ce 12 Timer tmr1, tmr2,tmr;
ikuta 4:00ac3d093b24 13 Timeout timeouter1,timeouter2;
ikuta 0:5d1796a0f9ce 14 int flag=0;
ikuta 4:00ac3d093b24 15 float sum1=0,sum2=0,count1=0,count2=0,timecount1=0,timecount2=0,timesum1=0,timesum2=0,v,argv;
ikuta 4:00ac3d093b24 16 int16_t x=0,y=0,i=2;
ikuta 4:00ac3d093b24 17 int8_t button = 0;
ikuta 0:5d1796a0f9ce 18 long microsecondsToCentimeters(long microseconds);
ikuta 0:5d1796a0f9ce 19
ikuta 0:5d1796a0f9ce 20 void reset()
ikuta 0:5d1796a0f9ce 21 {
ikuta 0:5d1796a0f9ce 22 flag = 0;
ikuta 0:5d1796a0f9ce 23 tmr.reset();
ikuta 2:fed59450f2ed 24 sum1=0,sum2=0,count1=0,count2=0,timecount1=0,timecount2=0,timesum1=0,timesum2=0;
ikuta 0:5d1796a0f9ce 25 }
ikuta 0:5d1796a0f9ce 26
ikuta 4:00ac3d093b24 27 void output()
ikuta 4:00ac3d093b24 28 {
ikuta 4:00ac3d093b24 29 printf("\n|v|: %lf theta: %lf\n",v,argv); //最終的にだすやつ
ikuta 4:00ac3d093b24 30 }
ikuta 4:00ac3d093b24 31
ikuta 0:5d1796a0f9ce 32 int main()
ikuta 0:5d1796a0f9ce 33 {
ikuta 4:00ac3d093b24 34 float avg1,avg2,timeavg1,timeavg2,deltat,a,b;
ikuta 0:5d1796a0f9ce 35 pc.baud(115200);
ikuta 0:5d1796a0f9ce 36 while (1) {
ikuta 2:fed59450f2ed 37 long duration1, duration2,USSDistance1, USSDistance2, cm1, cm2, time1,time2;
ikuta 0:5d1796a0f9ce 38
ikuta 0:5d1796a0f9ce 39 pingPin1.output();
ikuta 0:5d1796a0f9ce 40 pingPin1 = 0;
ikuta 0:5d1796a0f9ce 41 wait_us(2);
ikuta 0:5d1796a0f9ce 42 pingPin1 = 1;
ikuta 0:5d1796a0f9ce 43 wait_us(5);
ikuta 0:5d1796a0f9ce 44 pingPin1 = 0;
ikuta 0:5d1796a0f9ce 45
ikuta 0:5d1796a0f9ce 46 pingPin1.input(); // pulseIn
ikuta 0:5d1796a0f9ce 47 tmr1.reset();
ikuta 0:5d1796a0f9ce 48
ikuta 0:5d1796a0f9ce 49 while (!pingPin1); // wait for high
ikuta 0:5d1796a0f9ce 50 tmr1.start();
ikuta 0:5d1796a0f9ce 51 while (pingPin1); // wait for low
ikuta 0:5d1796a0f9ce 52 tmr1.stop();
ikuta 0:5d1796a0f9ce 53 duration1 = tmr1.read_us();
ikuta 0:5d1796a0f9ce 54 USSDistance1 = duration1;// * 0.0170;
ikuta 0:5d1796a0f9ce 55
ikuta 0:5d1796a0f9ce 56 cm1 = microsecondsToCentimeters(USSDistance1);
ikuta 0:5d1796a0f9ce 57
ikuta 0:5d1796a0f9ce 58 if(15<cm1&&cm1<40) {
ikuta 0:5d1796a0f9ce 59 if(flag==0) {
ikuta 0:5d1796a0f9ce 60 flag=1;
ikuta 0:5d1796a0f9ce 61 tmr.reset();
ikuta 0:5d1796a0f9ce 62 tmr.start();
ikuta 4:00ac3d093b24 63 timeouter2.attach(&output, 1);
ikuta 4:00ac3d093b24 64 timeouter1.attach(&reset, 5);
ikuta 0:5d1796a0f9ce 65 }
ikuta 0:5d1796a0f9ce 66 if(flag==1) {
ikuta 4:00ac3d093b24 67 timeouter1.detach();
ikuta 4:00ac3d093b24 68 timeouter2.attach(&output, 1);
ikuta 4:00ac3d093b24 69 timeouter1.attach(&reset, 5);
ikuta 0:5d1796a0f9ce 70 }
ikuta 2:fed59450f2ed 71 time1=tmr.read_ms();
ikuta 2:fed59450f2ed 72 printf("1: %d %d[ms]", cm1,time1);
ikuta 1:5aa590d1f1bf 73
ikuta 1:5aa590d1f1bf 74 count1++;
ikuta 2:fed59450f2ed 75 timecount1++;
ikuta 1:5aa590d1f1bf 76
ikuta 1:5aa590d1f1bf 77 sum1=sum1+cm1; //cm1の値を合計する
ikuta 1:5aa590d1f1bf 78 avg1=sum1/count1; //cm1の平均
ikuta 4:00ac3d093b24 79 printf(" avg1: %f",avg1);
ikuta 4:00ac3d093b24 80
ikuta 2:fed59450f2ed 81 timesum1=timesum1+time1; //time1を合計する
ikuta 2:fed59450f2ed 82 timeavg1=timesum1/timecount1; //time1の平均
ikuta 4:00ac3d093b24 83 printf(" timeavg1: %f[ms]\n",timeavg1);
ikuta 0:5d1796a0f9ce 84 }
ikuta 0:5d1796a0f9ce 85
ikuta 0:5d1796a0f9ce 86 wait_ms(10);
ikuta 0:5d1796a0f9ce 87
ikuta 0:5d1796a0f9ce 88 pingPin2.output();
ikuta 0:5d1796a0f9ce 89 pingPin2 = 0;
ikuta 0:5d1796a0f9ce 90 wait_us(2);
ikuta 0:5d1796a0f9ce 91 pingPin2 = 1;
ikuta 0:5d1796a0f9ce 92 wait_us(5);
ikuta 0:5d1796a0f9ce 93 pingPin2 = 0;
ikuta 0:5d1796a0f9ce 94
ikuta 0:5d1796a0f9ce 95 pingPin2.input(); // pulseIn
ikuta 0:5d1796a0f9ce 96 tmr2.reset();
ikuta 0:5d1796a0f9ce 97
ikuta 0:5d1796a0f9ce 98 while (!pingPin2); // wait for high
ikuta 0:5d1796a0f9ce 99 tmr2.start();
ikuta 0:5d1796a0f9ce 100 while (pingPin2); // wait for low
ikuta 0:5d1796a0f9ce 101 tmr2.stop();
ikuta 0:5d1796a0f9ce 102
ikuta 0:5d1796a0f9ce 103 duration2 = tmr2.read_us();
ikuta 0:5d1796a0f9ce 104 USSDistance2 = duration2;// * 0.0170;
ikuta 0:5d1796a0f9ce 105
ikuta 0:5d1796a0f9ce 106 cm2 = microsecondsToCentimeters(USSDistance2);
ikuta 0:5d1796a0f9ce 107 if(15<cm2&&cm2<40) {
ikuta 2:fed59450f2ed 108 time2=tmr.read_ms();
ikuta 4:00ac3d093b24 109 printf(" 2: %d %d[ms]",cm2,time2);
ikuta 1:5aa590d1f1bf 110
ikuta 1:5aa590d1f1bf 111 count2++;
ikuta 2:fed59450f2ed 112 timecount2++;
ikuta 4:00ac3d093b24 113
ikuta 1:5aa590d1f1bf 114 sum2=sum2+cm2; //cm2の値を合計する
ikuta 1:5aa590d1f1bf 115 avg2=sum2/count2; //cm2の平均
ikuta 4:00ac3d093b24 116 printf(" avg2: %f",avg2);
ikuta 4:00ac3d093b24 117
ikuta 2:fed59450f2ed 118 timesum2=timesum2+time2; //time2を合計する
ikuta 2:fed59450f2ed 119 timeavg2=timesum2/timecount2; //time2の平均
ikuta 4:00ac3d093b24 120 printf(" timeavg2:%f[ms]\n",timeavg2);
ikuta 4:00ac3d093b24 121
ikuta 3:814cb36d0f90 122 deltat=timeavg2-timeavg1; //Δtを求める
ikuta 4:00ac3d093b24 123 printf("\ndeltat: %f\n",deltat);
ikuta 4:00ac3d093b24 124
ikuta 3:814cb36d0f90 125 a=pow((avg2-avg1)/deltat,2);
ikuta 3:814cb36d0f90 126 b=pow(10/deltat,2);
ikuta 3:814cb36d0f90 127 v=sqrt(a+b); //|v|を求める
ikuta 4:00ac3d093b24 128
ikuta 3:814cb36d0f90 129 argv=atan(10/(avg2-avg1)); //θを求める
ikuta 0:5d1796a0f9ce 130 }
ikuta 0:5d1796a0f9ce 131 wait_ms(10);
ikuta 1:5aa590d1f1bf 132 }
ikuta 4:00ac3d093b24 133 button = (i >> 8) & 0x0F;
ikuta 4:00ac3d093b24 134 x = argv;
ikuta 4:00ac3d093b24 135 y = v;
ikuta 4:00ac3d093b24 136
ikuta 4:00ac3d093b24 137 joystick.update(x, y, button);
ikuta 0:5d1796a0f9ce 138 }
ikuta 0:5d1796a0f9ce 139
ikuta 0:5d1796a0f9ce 140 long microsecondsToInches(long microseconds)
ikuta 0:5d1796a0f9ce 141 {
ikuta 0:5d1796a0f9ce 142 return microseconds / 74 / 2;
ikuta 0:5d1796a0f9ce 143 }
ikuta 0:5d1796a0f9ce 144
ikuta 0:5d1796a0f9ce 145 long microsecondsToCentimeters(long microseconds)
ikuta 0:5d1796a0f9ce 146 {
ikuta 0:5d1796a0f9ce 147 return microseconds / 29 / 2;
ikuta 0:5d1796a0f9ce 148 }