Program to test and calibrate servomotors with arduino

Share your advanced PureBasic knowledge/code with the community.
Post Reply
User avatar
Psychophanta
Always Here
Always Here
Posts: 5153
Joined: Wed Jun 11, 2003 9:33 pm
Location: Anare
Contact:
Contact Psychophanta

Program to test and calibrate servomotors with arduino

Post by Psychophanta »

For the use with normal USB cable or bluetooth adapter installed in the microcontroller.
In case use a USB cable be aware to separate de 5volts power for servomotor, because de USB cable provides the Vin 5volts for the arduino.

Code: Select all

; _____*  Programa para arduino para probar motores mediante el protocolo de control clásico RC *_____
CompilerIf 0
#include <Servo.h>
Servo servo0,servo1;
float datuno,dat0uno; // <- para contener el dato de interes que entra
float datdos,dat0dos; // <- para contener el dato de interes que entra
bool tipodato0,tipodato1;
uint32_t datbau=115200,dat0bau=115200; // <- se pone 115200, pero depende del proyecto en cuestion
#define cadenalen 120
String cadena=""; // <- cadena para contener los datos que entran
String ini="ini"; // <- cadena para identificar el inicio de los datos que entran
String fin="fin"; // <- cadena para identificar el final de los datos que entran
String bau="bau"; // <- cadena para identificar baudios
String uno="uno"; // <- cadena para identificar variable uno
String dos="dos"; // <- cadena para identificar variable dos
String d0="d0"; // <- cadena para identificar si son grados
String d1="d1"; // <- cadena para identificar si son microsegundos
char inChar;
uint16_t servomin0,servomed0,servomax0;
uint16_t servomin1,servomed1,servomax1;
void setup()
{
  cadena.reserve(cadenalen);
  // inicializar comunicacion serial
  Serial.begin(datbau);
  while (!Serial); // espera para la enumeracion en el Leonardo, en los demas continua de inmediato
  while (Serial.available()&&Serial.read());Serial.flush(); // <- vaciar buffer
  delay(40);
  servomin0=600;servomed0=1500;servomax0=2400;
  servomin1=600;servomed1=1500;servomax1=2400;
  tipodato0=1;tipodato1=0; // 1 => microsegundos, 0 => grados
  datuno=servomed0;
  datdos=90;
  servo0.attach(10,servomin0,servomax0); // <- enganchar Servo0: marrón o negro -> masa, rojo -> 5v, amarillo o naranja o blanco -> D10, D5, D6... elige el que quieras
  servo1.attach(11,servomin1,servomax1); // <- enganchar Servo1: marrón o negro -> masa, rojo -> 5v, amarillo o naranja o blanco -> D10, D5, D6... elige el que quieras
  servo0.write(90);
  servo1.write(90);
}
void loop()
{
  if (Serial.available()>0) {
    inChar=(char)Serial.read(); // <- esto te lee todo, no se escapa ni un byte
    (cadena.length()>=cadenalen)?cadena=(String)inChar:cadena+=(String)inChar;
    if (cadena.indexOf(ini,0)>=0 && cadena.endsWith(fin)) {
      cadena.remove(cadena.indexOf(fin,0),fin.length());
      cadena.remove(0,cadena.indexOf(ini,0)+ini.length());
      if (cadena.indexOf(uno,0)==0) {
        cadena.remove(0,uno.length());
        dat0uno=cadena.toFloat();
        if (dat0uno!=datuno) {
          datuno=dat0uno;
          if (tipodato0) {
            servo0.writeMicroseconds(datuno);
            Serial.print(datuno);Serial.println(" microsegundos");
          }
          else {
            servo0.write(datuno);
            Serial.print(datuno);Serial.println(" grados");
          }
          Serial.flush();
        }
      }
      else if (cadena.indexOf(dos,0)==0) {
        cadena.remove(0,dos.length());
        dat0dos=cadena.toFloat();
        if (dat0dos!=datdos) {
          datdos=dat0dos;
          if (tipodato1) {
            servo1.writeMicroseconds(datdos);
            Serial.print(datdos);Serial.println(" microsegundos");
          }
          else {
            servo1.write(datdos);
            Serial.print(datdos);Serial.println(" grados");
          }
          Serial.flush();
        }
      }
      else if (cadena.indexOf(d0,0)==0) {
        cadena.remove(0,d0.length());
        if (cadena=="u") {
          datuno=servo0.readMicroseconds();
          Serial.print(datuno);Serial.println(" microsegundos");
          Serial.flush();
          tipodato0=1;
        }
        else if (cadena=="g") {
          datuno=servo0.read();
          Serial.print(datuno);Serial.println(" grados");
          Serial.flush();
          tipodato0=0;
        }
      }
      else if (cadena.indexOf(d1,0)==0) {
        cadena.remove(0,d1.length());
        if (cadena=="u") {
          datdos=servo1.readMicroseconds();
          Serial.print(datdos);Serial.println(" microsegundos");
          Serial.flush();
          tipodato1=1;
        }
        else if (cadena=="g") {
          datdos=servo1.read();
          Serial.print(datdos);Serial.println(" grados");
          Serial.flush();
          tipodato1=0;
        }
      }
      else if (cadena.indexOf(bau,0)==0) {
        cadena.remove(0,bau.length());
        dat0bau=cadena.toInt();
        if (dat0bau!=datbau) {
          datbau=dat0bau;
          while (Serial.available()&&Serial.read());Serial.flush(); // <- vaciar buffer
          Serial.end();
          delay(40);
          Serial.begin(datbau);
          while (!Serial); // espera para la enumeracion en el Leonardo, en los demas continua de inmediato
          Serial.print("baudios: ");Serial.println(datbau);
          delay(40);
          Serial.flush();
        }
      }
      else {
        (tipodato0)?servo0.writeMicroseconds(datuno):servo0.write(datuno);
        (tipodato1)?servo1.writeMicroseconds(datdos):servo1.write(datdos);
      }
      cadena="";
    }
  }
  //delay(10);
}
CompilerEndIf

Global NewList puertosRS232$()
Global PuertoCOM$="",baudios.l
Global cantidad.q; <- número de bytes reportados como disponibles para leer
Enumeration
  #ventana
  #cuadro
  #valor0
  #valor1
  #botonmotor0
  #grad_usec0
  #botonmotor1
  #grad_usec1
  #puertosCOM
  #baudios
  #deslizador0
  #deslizador1
  #serie=0
  #WindowWidth=390
  #WindowHeight=130
EndEnumeration

Procedure.a EnumSerial(List puertos$())
  Protected i.a
  For i=1 To 255
    If QueryDosDevice_("COM"+Str(i),Space(128),128)
      AddElement(puertos$()):puertos$()="COM"+Str(i)
    EndIf
  Next
  ProcedureReturn ListSize(puertos$())
EndProcedure
Procedure vaciarserial(puerto.a=#serie)
  Protected *nserie.byte,*serie.byte=AllocateMemory(10,#PB_Memory_NoClear)
  If *serie
    cantidad=AvailableSerialPortInput(puerto)
    While cantidad
      *nserie.byte=ReAllocateMemory(*serie.byte,cantidad,#PB_Memory_NoClear):*serie=*nserie
      ReadSerialPortData(puerto,*serie,cantidad)
      cantidad=AvailableSerialPortInput(puerto)
    Wend
    FreeMemory(*serie.byte)
    If *nserie And *serie<>*nserie:FreeMemory(*nserie.byte):EndIf
  EndIf
EndProcedure
Procedure.d EscalarRango(V.d,InputRangeL.d,InputRangeR.d,OutputRangeL.d,OutputRangeR.d)
  ;Interpola una variable V contenida dentro del rango no exponencial [InputRangeL,InputRangeR] hacia otro rango no exponencial [OutputRangeL,OutputRangeR]
  ;Util por ejemplo para barra de desplazamiento: TrackBarGadget()
  If V<InputRangeL:V=InputRangeL:EndIf
  If V>InputRangeR:V=InputRangeR:EndIf
  ProcedureReturn OutputRangeL+(V-InputRangeL)*((OutputRangeR-OutputRangeL)/(InputRangeR-InputRangeL))
EndProcedure

DataSection
  valoresbaudios:
  Data.l 300,1200,2400,4800,9600,19200,38400,57600,74880,115200,230400,250000,500000,1000000,2000000
EndDatasection
If EnumSerial(puertosRS232$())=0:MessageRequester("Error","No existen puertos RS232 disponibles"):End:EndIf

Global bloque.u=1,leido.q,cantidad.q,faltaba.u,ini.l,fin.l,uno.l,dos.l,dat.l,valor.l,var0.f,var1.f,*baseserie.byte=AllocateMemory(80)
Global.u servomin0=600,servomed0=1500,servomax0=2400,servomin1=600,servomed1=1500,servomax1=2400
OpenWindow(#ventana,400,300,#WindowWidth,#WindowHeight,"Ajustar calibrado de motor",#PB_Window_MinimizeGadget)
Top=5
GadgetHeight=24
FrameGadget(#cuadro,5,Top,380,120,"Ajustar calibrado de motor"):Top+20
ButtonGadget(#botonmotor0,10,Top,30,GadgetHeight,"m1",#PB_Button_Toggle):GadgetToolTip(#botonmotor0,"motor0 servo conectado en el pin #10")
ButtonGadget(#grad_usec0,40,Top,24,GadgetHeight,"us",#PB_Button_Toggle):GadgetToolTip(#grad_usec0,"grados o microsegundos del motor0")
SetGadgetState(#grad_usec0,1)
StringGadget(#valor0,65,Top,50,GadgetHeight,"")
ButtonGadget(#botonmotor1,116,Top,30,GadgetHeight,"m2",#PB_Button_Toggle):GadgetToolTip(#botonmotor1,"motor1 servo conectado en el pin #11")
ButtonGadget(#grad_usec1,146,Top,24,GadgetHeight,"°",#PB_Button_Toggle):GadgetToolTip(#grad_usec1,"grados o microsegundos del motor1")
StringGadget(#valor1,170,Top,50,GadgetHeight,"")
ComboBoxGadget(#puertosCOM,220,Top,60,GadgetHeight); puerto COM
ForEach puertosRS232$():AddGadgetItem(#puertosCOM,-1,puertosRS232$()):Next
ComboBoxGadget(#baudios,280,Top,100,GadgetHeight)
For s.a=1 To 15:Read.l a.l:AddGadgetItem(#baudios,-1,Str(a)+" baudio"):Next
SetGadgetState(#baudios,9):baudios.l=115200; <- 115200 baudios
TrackBarGadget(#deslizador0,10,60,370,25,0,1800):SetGadgetState(#deslizador0,900):var0.f=GetGadgetState(#deslizador0):SetGadgetText(#valor0,StrF(servomin0+var0,1)+"us")
TrackBarGadget(#deslizador1,10,90,370,25,0,1800):SetGadgetState(#deslizador1,900):var1.f=GetGadgetState(#deslizador1):SetGadgetText(#valor1,StrF(var1/10,1)+"°")
DisableGadget(#valor0,GetGadgetState(#botonmotor0)!1):DisableGadget(#deslizador0,GetGadgetState(#botonmotor0)!1):GadgetToolTip(#botonmotor0,"motor servo conectado en el pin #10")
DisableGadget(#valor1,GetGadgetState(#botonmotor1)!1):DisableGadget(#deslizador1,GetGadgetState(#botonmotor1)!1):GadgetToolTip(#botonmotor1,"motor servo conectado en el pin #11")
PokeS(@ini.l,"ini",3,#PB_Ascii):PokeS(@fin.l,"fin",3,#PB_Ascii)
PokeS(@uno.l,"uno",3,#PB_Ascii):PokeS(@dos.l,"dos",3,#PB_Ascii)
PokeS(@bau,"bau",3,#PB_Ascii)
Macro enviaryrecibirdato(gad,cual,dato,signo)
  If PuertoCOM$
    Copymemory(@ini,*baseserie,3):Copymemory(@cual#,*baseserie+3,3)
    i.a=StringByteLength(StrF(dato#,1),#PB_Ascii)
    PokeS(*baseserie+6,StrF(dato#,1),i,#PB_Ascii)
    Copymemory(@fin,*baseserie+6+i,3)
    WriteSerialPortData(#serie,*baseserie,9+i)
    Delay(10)
    cantidad=AvailableSerialPortInput(#serie)
    If cantidad
      leido=ReadSerialPortData(#serie,*baseserie,cantidad)
      dato#=ValF(PeekS(*baseserie,4,#PB_Ascii))
    EndIf
  EndIf
EndMacro
Macro deslizador(num,let)
  If GetGadgetState(#grad_usec#num#)
    var#num#.f=GetGadgetState(#deslizador#num#)+servomin#num#
    enviaryrecibirdato(#valor#num#,let#,var#num#.f,"us")
    SetGadgetText(#valor#num#,StrF(var#num#,1)+"us")
  Else
    var#num#.f=GetGadgetState(#deslizador#num#)/10.0
    enviaryrecibirdato(#valor#num#,let#,var#num#.f,"gr")
    SetGadgetText(#valor#num#,StrF(var#num#,1)+"°")
  EndIf
EndMacro
Macro valor(num,let)
  var#num#.f=ValF(GetGadgetText(#valor#num#))
  If GetGadgetState(#grad_usec#num#)
    enviaryrecibirdato(#valor0,let#,var#num#.f,"us")
    SetGadgetText(#valor#num#,StrF(var#num#,1)+"us")
    SetGadgetState(#deslizador#num#,var#num#-servomin#num#)
  Else
    enviaryrecibirdato(#valor#num#,let#,var#num#.f,"gr")
    SetGadgetText(#valor#num#,StrF(var#num#,1)+"°")
    SetGadgetState(#deslizador#num#,var#num#*10)
  EndIf
EndMacro
Macro tipodedato(num)
  cantidad=0
  If PuertoCOM$
    If GetGadgetState(#grad_usec#num#)
      PokeS(@dat.l,"d"+str(num#)+"u",3,#PB_Ascii)
    Else
      PokeS(@dat.l,"d"+str(num#)+"g",3,#PB_Ascii)
    EndIf
    WriteSerialPortData(#serie,@ini,3)
    WriteSerialPortData(#serie,@dat,3)
    WriteSerialPortData(#serie,@fin,3)
    Delay(10)
    cantidad=AvailableSerialPortInput(#serie)
    If cantidad
      leido=ReadSerialPortData(#serie,*baseserie,cantidad)
      var#num#.f=ValF(PeekS(*baseserie,4,#PB_Ascii))
      If GetGadgetState(#grad_usec#num#)
        SetGadgetText(#valor#num#,StrF(var#num#,1)+"us")
        SetGadgetState(#deslizador#num#,var#num#-servomin#num#)
        SetGadgetText(#grad_usec#num#,"us")
      Else
        SetGadgetText(#valor#num#,StrF(var#num#,1)+"°")
        SetGadgetState(#deslizador0,var#num#*10)
        SetGadgetText(#grad_usec#num#,"°")
      EndIf
    EndIf
  EndIf
  If cantidad=0:SetGadgetState(#grad_usec#num#,GetGadgetState(#grad_usec#num#)!1):EndIf
EndMacro
Repeat
  Repeat
    evento.i=WaitWindowEvent()
    Select evento
    Case #PB_Event_CloseWindow:Break 2
    Case #PB_Event_Gadget
      Select EventGadget()
      Case #baudios,#puertosCOM
        baudios.l=PeekL(?valoresbaudios+GetGadgetState(#baudios)*SizeOf(Long))
        PuertoCOM$=GetGadgetText(#puertosCOM)
        If PuertoCOM$
          While IsSerialPort(#serie)
            vaciarserial(#serie)
            CloseSerialPort(#serie)
          Wend
          ;Delay(1)
          If OpenSerialPort(#serie,PuertoCOM$,baudios,#PB_SerialPort_NoParity,8,0,#PB_SerialPort_NoHandshake,1024,8)
            SetGadgetText(#cuadro,"Ajustar calibrado de motor a través del puerto "+PuertoCOM$)
            vaciarserial(#serie)
            Delay(200)
            WriteSerialPortData(#serie,@ini,3)
            WriteSerialPortData(#serie,@bau,3)
            i.a=StringByteLength(Str(baudios),#PB_Ascii):PokeS(*baseserie,Str(baudios),i,#PB_Ascii):WriteSerialPortData(#serie,*baseserie,i)
            WriteSerialPortData(#serie,@fin,3)
          Else
            SetGadgetText(#cuadro,"No puede abrirse el puerto "+PuertoCOM$)
          EndIf
        EndIf
      Case #botonmotor0
        DisableGadget(#valor0,GetGadgetState(#botonmotor0)!1):DisableGadget(#deslizador0,GetGadgetState(#botonmotor0)!1)
      Case #botonmotor1
        DisableGadget(#valor1,GetGadgetState(#botonmotor1)!1):DisableGadget(#deslizador1,GetGadgetState(#botonmotor1)!1)
      Case #deslizador0
        deslizador(0,uno)
      Case #deslizador1
        deslizador(1,dos)
      Case #valor0
        valor(0,uno)
      Case #valor1
        valor(1,dos)
      Case #grad_usec0
        tipodedato(0)
      Case #grad_usec1
        tipodedato(1)
      EndSelect
    EndSelect
  Until evento=#PB_Event_None
  Delay(16)
Forever
If PuertoCOM$
  While IsSerialPort(#serie)
    vaciarserial(#serie)
    CloseSerialPort(#serie)
  Wend
EndIf
http://www.zeitgeistmovie.com

while (world==business) world+=mafia;
Top
User avatar
minimy
Enthusiast
Enthusiast
Posts: 552
Joined: Mon Jul 08, 2013 8:43 pm
Location: off world

Re: Program to test and calibrate servomotors with arduino

Post by minimy »

Interesting!!
I´ll try with my arduino and steeper motor.
Thanks!
If translation=Error: reply="Sorry, Im Spanish": Endif
Top
User avatar
Psychophanta
Always Here
Always Here
Posts: 5153
Joined: Wed Jun 11, 2003 9:33 pm
Location: Anare
Contact:
Contact Psychophanta

Re: Program to test and calibrate servomotors with arduino

Post by Psychophanta »

After closing the window, it brings out sometimes a memory access error at line 166:

Code: Select all

Protected *nserie.byte,*serie.byte=AllocateMemory(10,#PB_Memory_NoClear)
And I think it is a PB bug :o
http://www.zeitgeistmovie.com

while (world==business) world+=mafia;
Top
infratec
Always Here
Always Here
Posts: 7577
Joined: Sun Sep 07, 2008 12:45 pm
Location: Germany

Re: Program to test and calibrate servomotors with arduino

Post by infratec »

First: your logic with the buffers is a bit ... complicated.
I simplified it a bit:

Code: Select all

Procedure vaciarserial(puerto.i=#serie)
  
  Protected Byte.a
  
  If IsSerialPort(puerto)
    While AvailableSerialPortInput(puerto)
      ReadSerialPortData(puerto, @Byte, 1)
    Wend
  EndIf
  
EndProcedure
And the port needs to be an inzeger and not an ascii.

Also I think the problem comes from an other place.
Use the purifier with a granularity of 1,1,1,1.
Normally an overwritten memory location results in such a behaviour.
Top
infratec
Always Here
Always Here
Posts: 7577
Joined: Sun Sep 07, 2008 12:45 pm
Location: Germany

Re: Program to test and calibrate servomotors with arduino

Post by infratec »

Code: Select all

evento.i=WaitWindowEvent()
Is in your case totally wrong.

Code: Select all

evento.i=WindowEvent()
Is needed, else the repeat loop makes no sense.
Top
User avatar
Psychophanta
Always Here
Always Here
Posts: 5153
Joined: Wed Jun 11, 2003 9:33 pm
Location: Anare
Contact:
Contact Psychophanta

Re: Program to test and calibrate servomotors with arduino

Post by Psychophanta »

@infratec
Thanks! I will take a look again. That code has some years.
http://www.zeitgeistmovie.com

while (world==business) world+=mafia;
Top
Post Reply

Return to “Tricks 'n' Tips”