E:\Rob\stepmotor\ Última gravação: 20/10/ :18:18

Transcrição

1 Página:1/9 1: 'Stepmotor.bas 2: 'Programa para controle de motor de passo unipolar 3: 'através do UCN5804B - Translator/Driver da Allegro. 4: 'Utilizaremos o modo Half-Step, portanto o pino 9 do 5: 'UCN5804B deverá ser mantido baixo e o pino 10 alto. 6: 'No BX01 usaremos 4 pinos: um para alinhamento, um para 7: 'habilitar as saídas, um para a entrada dos pulsos de 8: 'meio-passo e um para a direção. 9: 'Para a comunicação com o sevidor utilizamos a porta 10: 'Com1 do BX01 a 19200bps com buffer de entrada e saída. 11: 'O microcontrolador recebe um comando na forma: 12: ' 13: ' <Par1;Par2;Par3;Par4;Par5> onde 14: ' Par1 = Ali (alinhamento automático) - 1 sim; 0 não 15: ' Par2 = Vel (velocidade) - 10 a 90 graus/segundo (de 10 em 10) 16: ' Par3 = Sen (Sentido) - 1 horário; 0 anti-horário 17: ' Par4 = Gir (Giros completos) - 0 a 5 (de 1 em 1) 18: ' Par5 = Gra (Posição final em graus) - 0 a 350 (de 10 em 10) 19: ' 20: 'executa esse comando e devolve K (ASCII) se OK. Se Houver erro, 21: 'devolve I (ASCII) e se reinicia. 22: ' 23: ' 24: 'Comandos: 25: ' 26: ' Align Alinha o eixo do motor com a referência, girando 27: ' no sentido horário. 28: ' TurnStep Gira o motor um determinado número de half-steps a 29: ' uma certa velocidade, num determinado sentido, 30: ' com time-out. 31: ' 32: 33: Const Vmax As Single = 15.0 'Velocidade máxima em rpm. 15rpm = 90g/s 34: Const PinR As Byte = 3 'Pino para referência (1=não referência, 0=referência) 35: Const PinS As Byte = 16 'Pino para a entrada dos pulsos de passo 36: Const PinD As Byte = 17 'Pino para a Direção (1 = Horário, 0 = Anti-Horário 37: Const PinE As Byte = 15 'Pino enable (0)/disable (1) 38: Const HspR As Single = 'Número de Meio-passos por rotação. Dep do motor 39: '

2 Página:2/9 40: Const InputBufferSize As Integer = 29 ' 20-byte buffer. 41: Const OutputBufferSize As Integer = 10 ' 1-byte buffer. 42: Dim InputBuffer(1 To InputBufferSize) As Byte 'Buffer de entrada 43: Dim OutputBuffer(1 To OutputBufferSize) As Byte 'Buffer de saída 44: Dim Param(1 To 5) As Integer 'Parâmetros do comando 45: Dim Patual As Integer 'Posição após execução de um comando 46: ' 47: 48: Sub Main() 49: 'Este programa principal é um loop infinito. 50: 'Ouve continuamente a porta serial para receber os comandos e executá-los. 51: 'Se não consegue ler os parâmetros, devolve I (ASCII) e reinicia o processador. 52: 'Se não consegue executar o comando, idem. 53: 'Se tudo OK, devolve K (ASCII) e volta a ouvir a porta serial. 54: 55: PutPin 14,bxOutputHigh 'Desativa rede : OpenQueue InputBuffer,InputBufferSize 'Abre buffer de entrada 57: OpenQueue OutputBuffer,OutputBufferSize 'Abre buffer de saída 58: OpenCom 1,19200,InputBuffer,OutputBuffer 'Abre a Com1 59: Disable 'Desabilita saidas motor 60: 61: Do 62: If Not GetPar Then 'Pega os 5 parâmetros do comando 63: Devolve 73,True 64: End If 65: If Not ExecCmd Then 'OK, Executa o comando. 66: Devolve 73,True 67: Else 68: Devolve 75,False 69: End If 70: Loop 'cicla sempre 71: End Sub 72: ' 73: 74: Function GetPar() As Boolean 75: 'Rotina para pegar os 5 parâmetros do comando que está no buffer de entrada 76: 77: Dim Cod As Byte 'Byte lido 78: Dim n As Byte 'Número de digitos

3 Página:3/9 79: Dim k As Integer 'Multiplicador 80: Dim Prm As Integer 'Parâmetro 81: Dim P(1 To 3) As Integer 'Cada digito 82: Dim Npar As Byte 'Número do parâmetro 83: 84: If Not ReadByte(Cod,0) Then 'Pega < 85: GetPar = False 'Não conseguiu 86: Exit Function 87: End If 88: Npar = 1 'Primeiro parâmetro 89: Do While(Npar <= 5) 'Ciclo para pegar os cinco parâmetros do comando 90: Prm = 0 'Zera parâmetro 91: Select Case Npar 92: Case 1,2,3,4 'Primeiros quatro parâmetros 93: n = 0 'Inicializa número de digitos 94: If Not ReadByte(Cod,5) Then 'Pega próximo caractere 95: GetPar = False 'Não conseguiu 96: Exit Function 97: End If 98: Do While(Cod <> 59) 'Cicla enquanto diferente de ; 99: n = n + 1 'Próximo digito 100: P(n) = CInt(Cod - 48) 'Pega cada digito 101: If Not ReadByte(Cod,5) Then 'Pega próximo caractere 102: GetPar = False 'Não conseguiu 103: Exit Function 104: End If 105: Loop 106: k = 1 'Inicializa multiplicador 107: Do While(n >= 1) 'Calcula o valor do parâmetro 108: Prm = Prm + (K * P(n)) 'Acumula cada digito 109: n = n - 1 'Próximo digito 110: k = k * 10 'Acerta multiplicador 111: Loop 112: Param(Npar) = Prm 'Armazena parâmetro 113: Case 5 'Quinto parâmetro 114: n = 0 'Inicializa número de digitos 115: If Not ReadByte(Cod,5) Then 'Pega próximo caractere 116: GetPar = False 'Não conseguiu 117: Exit Function

4 Página:4/9 118: End If 119: Do While(Cod <> 62) 'Cicla enquanto diferente de > 120: n = n + 1 'Próximo digito 121: P(n) = CInt(Cod - 48) 'Pega cada digito 122: If Not ReadByte(Cod,5) Then 'Pega próximo caractere 123: GetPar = False 'Não conseguiu 124: Exit Function 125: End If 126: Loop 127: k = 1 'Inicializa multiplicador 128: Do While(n >= 1) 'Calcula o valor do parâmetro 129: Prm = Prm + (K * P(n)) 'Acumula cada digito 130: n = n - 1 'Próximo digito 131: k = k * 10 'Acerta multiplicador 132: Loop 133: Param(Npar) = Prm 'Armazena parâmetro 134: End Select 135: Npar = Npar + 1 'Próximo parâmetro 136: Loop 137: GetPar = True 'OK, retorna True 138: ClearQueue InputBuffer 'Terminou, limpa buffer de entrada 139: End Function 140: ' 141: 142: Function ExecCmd() As Boolean 143: 'Param(1) => Ali (alinhamento automático) - 1 sim; 0 não 144: 'Param(2) => Vel (velocidade) - 10 a 90 graus/segundo (de 10 em 10) 145: 'Param(3) => Sen (Sentido) - 1 horário; 0 anti-horário 146: 'Param(4) => Gir (Giros completos) - 0 a 5 (de 1 em 1) 147: 'Param(5) => Gra (Posição final em graus) - 0 a 350 (de 10 em 10) 148: 149: Dim Tout As Single 'Time Out 150: Dim Rpm As Single 'Velocidade em rpm 151: Dim Rot As Single 'Número de rotações (decimal) a executar 152: Dim Value As Byte 'Byte 153: ' 154: If (Param(1) = 1) Then 'Execução com alinhamento automático, a partir de zero 155: Align 'Alinha eixo com a referência 156: Delay 0.5 'Dá um tempo

5 Página:5/9 157: If (Param(3) = 0) Then 'Sentido anti-horário 158: If (Param(5) = 0) Then 'Posição final igual a zero 159: Rot = 0.0 'Então rotação correspondente igual a zero 160: Else 'Posição final diferente de zero 161: Rot = CSng(360 - Param(5))/360.0 'Calcula fração de rotação 162: End If 163: Else 'Sentido horário 164: Rot = CSng(Param(5))/360.0 'Calcula fração de rotação 165: End If 166: If ((Rot = 0.0) And (CSng(Param(4)) = 0.0)) Then 'Bug do Basic Express 167: Patual = 0 168: ExecCmd = True 169: Exit Function 'Já está na posição, sai da rotina 170: End If 171: Rot = Rot + CSng(Param(4)) 'Acrescenta as rotações inteiras 172: Rpm = CSng(Param(2))/6.0 'Velocidade em rpm 173: Tout = 'Time-out máximo de 240 segundos 174: 'Executa comando e se não OK devolve falso 175: If Not TurnStep(CByte(Param(3)),Rpm,Rot,Tout) Then 'Executa comando 176: 'Não conseguiu 177: Patual = 0 178: ExecCmd = False 179: Exit Function 180: Else 181: 'Conseguiu 182: Patual = Param(5) 183: ExecCmd = True 184: Exit Function 185: End If 186: ' 187: Else 'Execução sem alinhamento, a partir da posição atual 188: If ((Param(5) > Patual) And (Param (3) = 1)) Then 189: 'Posição final maior que a atual no sentido horário 190: Rot = CSng(Param(4)) + (CSng(Param(5) - Patual)/360.0) 191: ElseIf ((Param(5) > Patual) And (Param(3) = 0)) Then 192: 'Posição final maior que a atual no sentido anti-horário 193: Rot = CSng(Param(4)) + ( CSng(Param(5)) + CSng(Patual))/ : ElseIf ((Param(5) < Patual) And (Param (3) = 1)) Then 195: 'Posição final menor que a atual no sentido horário

6 Página:6/9 196: If (Param(5) = 0) Then 197: 'Posição final = 0 198: Rot = CSng(Param(4)) + ( CSng(Patual))/ : Else 200: 'Posição final <> de 0 201: Rot = CSng(Param(4)) + ( CSng(Param(5)) - CSng(Patual))/ : End If 203: ElseIf ((Param(5) < Patual) And (Param(3) = 0)) Then 204: 'Posição final menor que a atual no sentido anti-horário 205: Rot = CSng(Param(4)) + (CSng(Patual) - CSng(Param(5)))/ : Else 207: 'Posição final igual à atual em qualquer sentido 208: Rot = CSng(Param(4)) 209: End If 210: Rpm = CSng(Param(2))/6.0 'Velocidade em rpm 211: Tout = 'Time-out máximo de 240 segundos 212: If Not TurnStep(CByte(Param(3)),Rpm,Rot,Tout) Then 'Executa comando 213: 'Não conseguiu 214: Patual = 0 215: ExecCmd = False 216: Exit Function 217: Else 218: 'Conseguiu 219: Patual = Param(5) 220: ExecCmd = True 221: Exit Function 222: End If 223: End If 224: End Function 225: ' 226: 227: Function ReadByte(ByRef Value As Byte, ByVal Tio As Byte) As Boolean 228: 'Esta função tenta ler um byte durante um tempo Tio. Se ela consegue ler o 229: 'byte num tempo < que Tio, ela devolve esse byte em Value e a função retorna 230: 'True. Caso contrario ela retorna False. 231: 232: Dim T1 As Single 'Instante inicial 233: Dim T2 As Single 'Instante presente 234:

7 Página:7/9 235: T1 = Timer 'Inicializa contagem de tempo 236: Do 237: T2 = Timer 'Pega este instante 238: If (Tio > 0) Then 'Verifica se é para verificar time-out 239: If ((T2 - T1) >= CSng(Tio)) Then 'Deu time-out sem conseguir ler nada 240: ReadByte = False 241: Exit Do 242: End If 243: End If 244: If (StatusQueue(InputBuffer)) Then 'Verifica se tem algo no buffer 245: GetQueue InputBuffer,Value,1 'Sim, então pega o caractere 246: ReadByte = True 247: Exit Do 'E sai 248: End If 249: Loop 'Cicla enquanto não chega algum caractere 250: End Function 251: ' 252: 253: Sub Align() 254: 'Rotina para alinhar o eixo do motor com a referência. Primeiro verifica se o 255: 'motor está em ponto cego. Se sim, gira 90g no sentido anti-horário a 10rpm. 256: 'Em seguida o motor começa a girar no sentido horário a uma velocidade 10rpm, 257: 'até que PinR se torne baixo. 258: 259: Dim Value As Byte 'Byte 260: 261: If (GetPin(PinR) = 0) Then 'Está em ponto cego 262: If Not TurnStep(0, 10.0, 0.25, 5.0) Then 'Gira 90g anti-horário 263: Devolve 73,True 264: Exit Sub 265: End If 266: End If 267: Delay 0.5 'Dá um tempo 268: PutPin PinD, 1 'Sentido horário 269: Enable 'Habilita saidas motor 270: Do 'Executa 271: PutPin PinS, 1 'Pulsa 272: PutPin PinS, 0 'PinS 273: Delay 'Aguarda DT => 10rpm = 60g/s

8 Página:8/9 274: Loop While GetPin(PinR) = 1 'Enquanto PinR = 1 275: Disable 'Desabilita saidas motor 276: End Sub 277: ' 278: 279: Function TurnStep(ByVal Sen As Byte, ByVal Vel As Single, _ 280: ByVal Nrot As Single, ByVal Tof As Single) As Boolean 281: 'Rotina para girar o motor um certo número de meio-passos a uma 282: 'determinada velocidade num determinado sentido, com time-out. 283: 'Sen: Sentido de giro / 1 - Horário, 0 - Anti-horário 284: 'Vel: Velocidade em rpm 285: 'Nrot: Número de rotações 286: 'Tof: Time-out em segundos 287: 288: Dim I As Integer 'Contador de meio-passos 289: Dim T1 As Single 'Instante inicial 290: Dim T2 As Single 'Instante presente 291: Dim DT As Single 'Tempo entre meio-passos 292: Dim Nstep As Integer 'Número de meio-passos a percorrer 293: 294: PutPin PinD, Sen 'Seta sentido 295: If Vel > Vmax Then 'Se velocidade pedida > Vmax 296: Vel = Vmax 'Então Velocidade = Vmax 297: End If 298: DT = 60.0 / (Vel * HspR) 'Calcula tempo entre meio-passos 299: Nstep = CInt(Nrot * HspR) 'Número de meio-passos a percorrer 300: T1 = Timer 'Instante inicial 301: Enable 'Habilita saidas motor 302: For I=1 To Nstep 'Conta o número de meio-passos 303: PutPin PinS, 1 'Pulsa 304: PutPin PinS, 0 'PinS 305: Delay DT 'Aguarda DT 306: T2 = Timer 'Pega este instante 307: if (T2 - T1) >= Tof Then 'Deu time-out 308: TurnStep = False 'Devolve falso 309: Disable 'Desabilita saidas motor 310: Exit Function 'Encerra 311: End If 312: Next

9 Página:9/9 313: TurnStep = True 'Devolve verdadeiro 314: Disable 'Desabilita saidas motor 315: End Function 316: ' 317: 318: Sub PutByte(ByVal Value As Byte) 319: 'Manda 1 byte para a serial 320: PutQueue OutputBuffer,Value,1 321: End Sub 322: ' 323: 324: Sub Enable() 325: PutPin PinE, 0 326: End Sub 327: ' 328: 329: Sub Disable() 330: PutPin PinE, 1 331: End Sub 332: ' 333: 334: Sub Devolve(ByVal Cod As Byte,ByVal Rst As Boolean) 335: 'Devolve Cod (em ASCII) e se reinicia se Rst = True 336: PutByte Cod 337: If Rst Then 338: ResetProcessor 'Reinicia BX01 339: end If 340: End Sub 341: ' 342: 343: