' ============================================================
' IMU 陀螺儀感應器 EV3 新手範例 04：走直線自動修正
' 軟體：clev3r
' 接法：IMU 陀螺儀插 1 號孔；左輪馬達接 B、右輪馬達接 C
' 這個範例會做什麼：兩個輪子不可能完全一樣快，車子直走久了會歪。用陀螺儀看歪了幾度，就往反方向補回來，車子就能走得很直。
' 讀值：angle＝機器人轉了幾度（連續累加（轉一圈＝360，可超過 360））
' ============================================================
folder "prjs" "MBC_imu_ex04"

' IMU 陀螺儀插 1 號孔，使用模式 0
Sensor.SetMode(1, 0)
' 陀螺儀歸零的指令（送 0x11 給感應器）
gyroReset[0] = 17
LCD.Clear()

' 角度歸零：現在的方向就是「正前方」
Sensor.SendUARTData(1, 1, gyroReset)
Program.Delay(100)
' 一直重複
While "True"
  ' 修正力量 fix ＝ 角度 × 2
  fix = Sensor.ReadRawValue(1, 0) * 2
  ' 往右歪（角度變大）：左輪減速、右輪加速，把車頭拉回來
  Motor.StartPower("B", 30 - fix)
  Motor.StartPower("C", 30 + fix)
EndWhile