Sub Calc1()
'
'******************
' 計算１
'******************
'
Form1.RetInf = ""
Form1.RetInf = "Calculating..."
'
Tr = 0.505 '後輪トレッド（幅)(m)
Rw = 0.31 '後輪半径(m)
'
' 最大値・最小値の初期化
'
Distance_min(2) = 0: Distance_max(2) = 0
Speed_max(0) = 0: Speed_max(1) = 0
Speed_min(0) = 0: Speed_min(1) = 0
Tq_max(0) = 0: Tq_max(1) = 0
Tq_min(0) = 0: Tq_min(1) = 0
Power_max(0) = 0: Power_max(1) = 0
Power_min(0) = 0: Power_min(1) = 0
x_min = 0: x_max = 0
y_min = 0: y_max = 0
Alpha_deg_min = 0: Alpha_deg_max = 0
Beta_deg_min = 0: Beta_deg_max = 0
Tq_av(0) = 0: Tq_av(1) = 0
Power_av(0) = 0:  Power_av(1) = 0
dSpeed(0) = 0: dSpeed(1) = 0: dSpeed(2) = 0
dTq(0) = 0: dTq(1) = 0
dPower(0) = 0: dPower(1) = 0
'
' 初期値(i=1)
'
Angle_deg(0, 1) = 0: Angle_deg(1, 1) = 0
Distance(0, 1) = 0: Distance(1, 1) = 0
If Form1.chkShift.Value = 1 Then '0Vの補正をする
  Tq(0, 1) = Ka(0, 1) * Volt(0, 1)
  Tq(1, 1) = Ka(1, 1) * Volt(1, 1)
Else
  Tq(0, 1) = Ka(0, 1) * Volt(0, 1) + Ka(0, 0)
  Tq(1, 1) = Ka(1, 1) * Volt(1, 1) + Ka(1, 0)
End If
Alpha(1) = 0
Beta(1) = 0
x(1) = 0: y(1) = 0
'
' データ処理
'
For i = 2 To DataNum
  '
  ' 車輪角度の計算
  '
  If Encoder(0, i) - Encoder(0, i - 1) < -500 Then '4095→0（逆回転）
    Angle_deg(0, i) = Angle_deg(0, i - 1) - (Encoder(0, i) + 4096 - Encoder(0, i - 1)) * 360 / 2000
  ElseIf Encoder(0, i) - Encoder(0, i - 1) > 500 Then '0→4095（正回転）
    Angle_deg(0, i) = Angle_deg(0, i - 1) - (Encoder(0, i) - 4096 - Encoder(0, i - 1)) * 360 / 2000
  Else
    Angle_deg(0, i) = Angle_deg(0, i - 1) - (Encoder(0, i) - Encoder(0, i - 1)) * 360 / 2000
  End If
  Angle(0, i) = Angle_deg(0, i) * 3.14159 / 180 '(rad)
  '
  If Encoder(1, i) - Encoder(1, i - 1) < -500 Then '4095→0（正回転）
    Angle_deg(1, i) = Angle_deg(1, i - 1) + (Encoder(1, i) + 4096 - Encoder(1, i - 1)) * 360 / 2000
  ElseIf Encoder(1, i) - Encoder(1, i - 1) > 500 Then '0→4095（逆回転）
  Angle_deg(1, i) = Angle_deg(1, i - 1) + (Encoder(1, i) - 4096 - Encoder(1, i - 1)) * 360 / 2000
  Else
    Angle_deg(1, i) = Angle_deg(1, i - 1) + (Encoder(1, i) - Encoder(1, i - 1)) * 360 / 2000
  End If
  Angle(1, i) = Angle_deg(1, i) * 3.14159 / 180 '(rad)
  '
  ' 移動距離・速度の計算
  '
  Distance(0, i) = Rw * Angle(0, i) '移動距離(m)
  Distance(1, i) = Rw * Angle(1, i) '移動距離(m)
  Distance(2, i) = (Distance(0, i) + Distance(1, i)) / 2 '左右の平均移動距離(m)
  Speed(0, i) = (Distance(0, i) - Distance(0, i - 1)) / (SampClk / 1000) '速度(m/s)
  Speed(1, i) = (Distance(1, i) - Distance(1, i - 1)) / (SampClk / 1000) '速度(m/s)
  Speed(2, i) = (Speed(0, i) + Speed(1, i)) / 2 '左右の平均速度(m/s)
  '
  ' トルク・出力の計算
  '
  If Form1.chkShift.Value = 1 Then '0Vの補正をする
    Tq(0, i) = Ka(0, 1) * Volt(0, i)
    Tq(1, i) = Ka(1, 1) * Volt(1, i)
  Else
    Tq(0, i) = Ka(0, 1) * Volt(0, i) + Ka(0, 0)
    Tq(1, i) = Ka(1, 1) * Volt(1, i) + Ka(1, 0)
  End If
  Power(0, i) = (Tq(0, i) + Tq(0, i - 1)) * (Angle(0, i) - Angle(0, i - 1)) / 2 / (SampClk / 1000) '出力(W)
  Power(1, i) = (Tq(1, i) + Tq(1, i - 1)) * (Angle(1, i) - Angle(1, i - 1)) / 2 / (SampClk / 1000) '出力(W)
  '
  ' 旋回角度，角速度の計算
  '
  dDistance(0) = Distance(0, i) - Distance(0, i - 1) '左車輪の1ステップの移動距離(m)
  dDistance(1) = Distance(1, i) - Distance(1, i - 1) '右車輪の1ステップの移動距離(m)
  If dDistance(0) = dDistance(1) Then '直進または不動
    Beta(i) = 0 '旋回角速度(rad/sec)
    Alpha(i) = Alpha(i - 1)
    Turn$ = "cent"
  ElseIf dDistance(0) = 0 Or dDistance(1) = 0 Then
    R = Tr / 2
    Beta(i) = (dDistance(1) - dDistance(0)) / Tr '角速度(rad/step)（左旋回が正，右旋回が負）
    Alpha(i) = Alpha(i - 1) + Beta(i) '旋回角度(rad)
    If Beta(i) > 0 Then Turn$ = "left"
    If Beta(i) < 0 Then Turn$ = "right"
  ElseIf dDistance(0) < dDistance(1) Then '左旋回
    R = (Tr / 2 * (dDistance(1) + dDistance(0))) / (dDistance(1) - dDistance(0)) '旋回半径(m)
    Beta(i) = dDistance(1) / (R + Tr / 2) '旋回角速度(rad/step)
    Alpha(i) = Alpha(i - 1) + Beta(i) '旋回角度(rad)
    Turn$ = "left"
  ElseIf dDistance(0) > dDistance(1) Then '右旋回
    R = (Tr / 2 * (dDistance(1) + dDistance(0))) / (dDistance(0) - dDistance(1)) '旋回半径(m)
    Beta(i) = -dDistance(0) / (R + Tr / 2) '旋回角速度(rad/step)
    Alpha(i) = Alpha(i - 1) + Beta(i) '旋回角度(rad)
    Turn$ = "right"
  End If
  '
  ' 軌跡座標の計算（y=横方向，x=進行方向）
  '
  If Turn$ = "cent" Then
    x(i) = x(i - 1) + dDistance(0) * Cos(Alpha(i))
    y(i) = y(i - 1) + dDistance(0) * Sin(Alpha(i))
  ElseIf Turn$ = "left" Then
    x(i) = x(i - 1) + R * Sin(Alpha(i)) - R * Sin(Alpha(i - 1)) '後輪中心位置の座標（左右方向）
    y(i) = y(i - 1) - R * Cos(Alpha(i)) + R * Cos(Alpha(i - 1)) '後輪中心位置の座標（進行方向）
  ElseIf Turn$ = "right" Then
    x(i) = x(i - 1) - R * Sin(Alpha(i)) + R * Sin(Alpha(i - 1)) '後輪中心位置の座標（左右方向）
    y(i) = y(i - 1) + R * Cos(Alpha(i)) - R * Cos(Alpha(i - 1)) '後輪中心位置の座標（進行方向）
  End If
  Beta_deg(i) = Beta(i) / (SampClk / 1000) * 180 / 3.14159 '旋回角速度(deg/sec)
  Alpha_deg(i) = Alpha(i) * 180 / 3.14159 '旋回角度(deg)
  '
  ' 最大値・最小値・平均値の計算
  '
  If Distance_min(2) > Distance(2, i) Then Distance_min(2) = Distance(2, i)
  If Distance_max(2) < Distance(2, i) Then Distance_max(2) = Distance(2, i)
  If Speed_min(0) > Speed(0, i) Then Speed_min(0) = Speed(0, i) '最小速度（仮）
  If Speed_min(1) > Speed(1, i) Then Speed_min(1) = Speed(1, i) '最小速度（仮）
  If Speed_max(0) < Speed(0, i) Then Speed_max(0) = Speed(0, i) '最大速度（仮）
  If Speed_max(1) < Speed(1, i) Then Speed_max(1) = Speed(1, i) '最大速度（仮）
  If Tq_min(0) > Tq(0, i) Then Tq_min(0) = Tq(0, i) '最小トルク（仮）
  If Tq_min(1) > Tq(1, i) Then Tq_min(1) = Tq(1, i) '最小トルク（仮）
  If Tq_max(0) < Tq(0, i) Then Tq_max(0) = Tq(0, i) '最大トルク（仮）
  If Tq_max(1) < Tq(1, i) Then Tq_max(1) = Tq(1, i) '最大トルク（仮）
  If Power_min(0) > Power(0, i) Then Power_min(0) = Power(0, i) '最小出力（仮）
  If Power_min(1) > Power(1, i) Then Power_min(1) = Power(1, i) '最小出力（仮）
  If Power_max(0) < Power(0, i) Then Power_max(0) = Power(0, i) '最大出力（仮）
  If Power_max(1) < Power(1, i) Then Power_max(1) = Power(1, i) '最大出力（仮）
  If x_min > x(i) Then x_min = x(i)
  If x_max < x(i) Then x_max = x(i)
  If y_min > x(i) Then y_min = x(i)
  If y_max < x(i) Then y_max = x(i)
  If Alpha_deg_min > Alpha_deg(i) Then Alpha_deg_min = Alpha_deg(i)
  If Alpha_deg_max < Alpha_deg(i) Then Alpha_deg_max = Alpha_deg(i)
  If Beta_deg_min > Beta_deg(i) Then Beta_deg_min = Beta_deg(i)
  If Beta_deg_max < Beta_deg(i) Then Beta_deg_max = Beta_deg(i)
  Tq_av(0) = Tq_av(0) + Tq(0, i)
  Tq_av(1) = Tq_av(1) + Tq(1, i)
  Power_av(0) = Power_av(0) + Power(0, i)
  Power_av(1) = Power_av(1) + Power(1, i)
Next i
'
Speed_av(0) = (Distance(0, DataNum) - Distance(0, 1)) / SampTime '平均速度(m/s)
Speed_av(1) = (Distance(1, DataNum) - Distance(1, 1)) / SampTime '平均速度(m/s)
Speed_av(2) = (Distance(2, DataNum) - Distance(2, 1)) / SampTime '平均速度(m/s)
If Speed_av(0) <> 0 Then
  dSpeed(0) = (Speed_max(0) - Speed_min(0)) / Speed_av(0) '速度変動率
End If
If Speed_av(1) <> 0 Then
  dSpeed(1) = (Speed_max(1) - Speed_min(1)) / Speed_av(1) '速度変動率
End If
If Speed_av(2) <> 0 Then
  dSpeed(2) = (Speed_max(2) - Speed_min(2)) / Speed_av(2) '速度変動率
End If
Tq_av(0) = Tq_av(0) / (DataNum - 1) '平均トルク(Nm)
Tq_av(1) = Tq_av(1) / (DataNum - 1) '平均トルク(Nm)
If Tq_av(0) <> 0 Then
  dTq(0) = (Tq_max(0) - Tq_min(0)) / Tq_av(0) 'トルク変動率
End If
If Tq_av(1) <> 0 Then
  dTq(1) = (Tq_max(1) - Tq_min(1)) / Tq_av(1) 'トルク変動率
End If
Power_av(0) = Power_av(0) / (DataNum - 1) '平均出力(Nm)
Power_av(1) = Power_av(1) / (DataNum - 1) '平均出力(Nm)
If Power_av(0) <> 0 Then
  dPower(0) = (Power_max(0) - Power_min(0)) / Power_av(0) 'エネルギ変動率
End If
If Power_av(1) <> 0 Then
  dPower(1) = (Power_max(1) - Power_min(1)) / Power_av(1) 'エネルギ変動率
End If
'
End Sub