超音波センサの方向から車体の方向を計算 a = 0.6 車軸の中点と超音波センサの回転軸との距離(cm) b = 9.2 車軸と超音波センサの回転軸との距離(cm) c = 9.1 超音波センサの回転軸から超音波センサの距離(cm) d = 5 超音波センサの値 d y0 c x0 = (d+c)*cos(θ) y0 = (d+c)*sin(θ) x1 = x0 + a y1 = y0 + b r = sqrt(x1*x1 + y1*y1) X = sgn(y1)*arccos(x1/r) θ y1 e b x g a x1 x0
2つの超音波センサの値から車体の方向を計算 a = 0.6 車軸の中点と超音波センサの回転軸との距離(cm) b = 9.2 車軸と超音波センサの回転軸との距離(cm) c = 9.1 超音波センサの回転軸から超音波センサの距離(cm) d = 5 超音波センサの値 c d v1 b v2 e = 車軸の中点と超音波センサとの距離(cm) f = 車軸と超音波センサとの距離(cm) g = 超音波センサの値 e a x f g v1 = (a+c+d)-(e+g) v2 = b+f tan(X) = v1/v2 X = arctan(v1/v2)
超音波センサの値から車体の右上を物体に近づける a = 0.6 車軸の中点と超音波センサの回転軸との距離(cm) b = 9.2 車軸と超音波センサの回転軸との距離(cm) c = 9.1 超音波センサの回転軸から超音波センサの距離(cm) d = 5 超音波センサの値 v1 d h = 車軸から前方までの距離(cm) i = 幅(cm) X v2 c v1 = (b+c+d)-h v2 = i/2 tan(X) = v1/v2 X = arctan(v1/v2) Y/v2 = cos(X) Y = v2*cos(X) h Y b a i
超音波センサの値から車体の右上を物体に近づける a = 0.6 車軸の中点と超音波センサの回転軸との距離(cm) b = 9.2 車軸と超音波センサの回転軸との距離(cm) c = 9.1 超音波センサの回転軸から超音波センサの距離(cm) d = 5 超音波センサの値 v1 d h = 車軸から前方までの距離(cm) i = 幅(cm) X v2 c v1 = (b+c+d)-h v2 = i/2 tan(X) = v1/v2 X = arctan(v1/v2) Y/v2 = cos(X) Y = v2*cos(X) h Y b a i
超音波センサの方向から車体の方向を計算 a = 0.6 車軸の中点と超音波センサの回転軸との距離(cm) b = 9.2 車軸と超音波センサの回転軸との距離(cm) c = 9.1 超音波センサの回転軸から超音波センサの距離(cm) d = 5 超音波センサの値 cos(θ) = j/k k = j/cos(θ) sin(θ) = l/k l=k/sin(θ) tan(θ) = l/j l = j * tan(θ) j k l θ j