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@8:59dd4860477f, 2016-09-27 (annotated)

Committer:
ikuta
Date:
Tue Sep 27 13:42:03 2016 +0000
Revision:
8:59dd4860477f
Parent:
7:ed304ba2712a
Child:
12:36a6bae22fbf
????????????

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