競速自走車~角度控制

1. Peripheral myM As RacerM1 @ 3
   
2. Peripheral myP As RacerP1 @ 4
   
3. 
   
4. Sub Main()
   
5. 
   
6.         InitMotor()
   
7.         Daul(250,100)            '前進(速度, 行進長度)
   
8.         TurnLeft(250,40)        '左轉(速度, 行進長度)
   
9.         Daul(250,100)            '前進(速度, 行進長度)
   
10.         TurnRight(250,40)      '右轉(速度, 行進長度)
    
11.         Daul(250,100)            '前進(速度, 行進長度)
    
12. 
    
13. End Sub
    
14. 
    
15. Sub TurnLeft(Speed As Word,Tach As Word)
    
16.         Dim Temp_R,Temp_Tach As Word
    
17.         Dim bStatus As Byte
    
18.         Temp_R = 0
    
19.         myM.SetStraight(Speed,0)
    
20.         Do
    
21.                 Do
    
22.                         bStatus = myP.TACHInR(Temp_Tach)
    
23.                 Loop Until bStatus = 1
    
24.                 Temp_R = Temp_R + Temp_Tach
    
25.         Loop Until Temp_R > Tach
    
26.         myM.SetStraight(0,0)
    
27. End Sub
    
28. 
    
29. Sub TurnRight(Speed As Word,Tach As Word)
    
30.         Dim Temp_L,Temp_Tach As Word
    
31.         Dim bStatus As Byte
    
32.         Temp_L = 0
    
33.         myM.SetStraight(0,Speed)
    
34.         Do
    
35.                 Do
    
36.                         bStatus = myP.TACHInL(Temp_Tach)
    
37.                 Loop Until bStatus = 1
    
38.                 Temp_L = Temp_L + Temp_Tach
    
39.         Loop Until Temp_L > Tach
    
40.         myM.SetStraight(0,0)
    
41. End Sub
    
42. 
    
43. Sub Daul(Speed As Word,Tach As Word)
    
44.         myM.SetStraight(Speed,Speed)
    
45.         Pause 5 * Tach
    
46.         myM.SetStraight(0,0)
    
47. End Sub
    
48. 
    
49. Sub InitMotor()                                  '設定M1模組各項預設值
    
50.     myM.SetP(80)                                '設定M1模組內部PID參數值
    
51.     myM.SetI(1)
    
52.     myM.SetD(80)
    
53.     myM.SetScalar(0)
    
54.         myM.SetIRThreshold(70)             ' 設定紅外線感測強度        
    
55.     myM.SetSpdCtrlA(-1024,1024)        '設定M1模組內部最高最低速限，以及直線速度
    
56.     myM.SetSpdCtrlB(-1024,1024)
    
57.         myM.SetStraight(0,0)
    
58.         myM.SetErrScale(0,0,0,0,0,0)'設定M1模組各別感測階段的誤差值
    
59.     myM.SetOutsideMode(0)
    
60.     myM.SpdCtrlOn(1)
    
61. End Sub

複製代碼