討論區 › 競速自走車 › 競速自走車~角度控制

robot-why 發表於 2011-1-26 16:37

競速自走車~角度控制

Peripheral myM As RacerM1 @ 3
Peripheral myP As RacerP1 @ 4

Sub Main()

      InitMotor()
      Daul(250,100)            '前進(速度, 行進長度)
      TurnLeft(250,40)      '左轉(速度, 行進長度)
      Daul(250,100)            '前進(速度, 行進長度)
      TurnRight(250,40)      '右轉(速度, 行進長度)
      Daul(250,100)            '前進(速度, 行進長度)

End Sub

Sub TurnLeft(Speed As Word,Tach As Word)
      Dim Temp_R,Temp_Tach As Word
      Dim bStatus As Byte
      Temp_R = 0
      myM.SetStraight(Speed,0)
      Do
                Do
                        bStatus = myP.TACHInR(Temp_Tach)
                Loop Until bStatus = 1
                Temp_R = Temp_R + Temp_Tach
      Loop Until Temp_R > Tach
      myM.SetStraight(0,0)
End Sub

Sub TurnRight(Speed As Word,Tach As Word)
      Dim Temp_L,Temp_Tach As Word
      Dim bStatus As Byte
      Temp_L = 0
      myM.SetStraight(0,Speed)
      Do
                Do
                        bStatus = myP.TACHInL(Temp_Tach)
                Loop Until bStatus = 1
                Temp_L = Temp_L + Temp_Tach
      Loop Until Temp_L > Tach
      myM.SetStraight(0,0)
End Sub

Sub Daul(Speed As Word,Tach As Word)
      myM.SetStraight(Speed,Speed)
      Pause 5 * Tach
      myM.SetStraight(0,0)
End Sub

Sub InitMotor()                                  '設定M1模組各項預設值
    myM.SetP(80)                              '設定M1模組內部PID參數值
    myM.SetI(1)
    myM.SetD(80)
    myM.SetScalar(0)
      myM.SetIRThreshold(70)             ' 設定紅外線感測強度      
    myM.SetSpdCtrlA(-1024,1024)      '設定M1模組內部最高最低速限，以及直線速度
    myM.SetSpdCtrlB(-1024,1024)
      myM.SetStraight(0,0)
      myM.SetErrScale(0,0,0,0,0,0)'設定M1模組各別感測階段的誤差值
    myM.SetOutsideMode(0)
    myM.SpdCtrlOn(1)
End Sub

查看完整版本: 競速自走車~角度控制