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.
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?
User | Revision | Line number | New 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 | } |