Steven Ortiz / Mbed 2 deprecated PICCOLO_CNC1

PROGRAMACION PARA PICCOLO

Dependencies:   mbed Move_Servos Step_Motor

Resumen:

Este trabajo está diseñado con el fin de crear un robot Piccolo que tiene designada la labor de realizar dibujos automáticos, siguiendo movimientos de los ejes X, Y,Z y guiándose en el funcionamiento de una CNC, los cuales le permitirán al robot realizar su movimientos y sus tareas. El Piccolo será de tipo inalámbrico y su conexión se hará por medo de via bluetooth, ya que los comandos son enviados al robot por medio de un computador externo al del sistema, pero por el momento se hacer por comunicación serial desde el computador y la tarjeta (cable), por último consiste en la evaluación del sistema y conocer los puntos a favor y en contra que deja la creación de este robot.

Objetivos:

Objetivo general.

1.

Desarrollar un sistema embebido específicamente un robot Piccolo, con el fin de cumplir tareas de dibujo automático.

Objetivo específico.

1.

Construir un diseño mecánico que se acomode con la función designada.

2.

Construir un sistema electrónico y de control que se ajuste con el diseño mecánico, y sean los encargados del funcionamiento del robot.

3.

Evaluar el prototipo en diferentes pruebas para determinar el cumplimiento con la necesidad planteada.

DISEÑO ELECTRÓNICO:

Para el diseño electrónico del prototipo se tendrá un circuito principal el cual es el encargado de tener todas las conexiones que el mismo necesita para su funcionamiento, dicho circuito tendrá sus diferentes componentes, como lo son resistencias, diodos led, y elementos principales como lo es:

Microcontrolador:

Se va a utilizar la placa de desarrollo STM32 con el núcleo F446 la cual admiten complementos de hardware con facilidad, permitiendo el acceso libre al compilador en línea mbed.

Servo Motor:

Se utilizarán los Servo Motores Mg90 Tower Pro ya que estos se acoplan fácilmente con el diseño de la estructura del prototipo, además su pwm permitirá un manejo exacto de cada movimiento de los ejes (x, y, z) que se van a controlar.

Motor Paso a Paso

Se va a utilizar los motores paso a paso 28BYJ-48 con su respectivo driver, y estos motores servirán para el movimiento entre cuadrantes.

PRIMER NIVEL

En este diagrama se muestra como está repartida la alimentación para cada componente, y cada uno de ellos recibe 5 voltios para su funcionamiento.

SEGUNDO NIVEL

Cada componente tiene su circuito interno, por este motivo se encuentra en el segundo nivel, ya que para que el tercer nivel funcione, necesita de este nivel dos. Entre los cuales tenemos los circuitos de la tarjeta de desarrollo STM32F446, y el circuito para el Servo motor.

TERCER NIVEL

En este nivel se encuentra las comunicaciones entre todos los componentes, que se comunica con que y bajo que estándar lo hacen. Entre la interfaz (pc) y la tarjeta de desarrollo, se comunican mediante UART, de la tarjeta de desarrollo a los servo motores lo hace por PWM (pulse-width modulation) y de la tarjeta a los motores paso a paso, por un bus de datos de 4 bits cada motor.

Programación

El programa esta orientado a el movimiento que debe realizar un PICCOLO CNC. Se realizo su programación orientada a objetos la cual se tuvo que realizar una librería. Esta librería cuenta con una cabecera la cual se llama Servo_X.h la cual contiene la programación;

-clase llamada servo:

en la cual tendrá encapsulado los valores x,y,z, los cuales corresponden a las posiciones actuales (en milímetros) en la cual se encuentran los servos. Además de ello, se encuentra mm2pulse, el cual estará encargado de la conversión de milímetros al pulso que recibirá el PWM. en esta misma clase se encuentra nuestro constructor llamado Servo(), nuestro destructor Servo(), nuestro SetServo (el cual recibirá las posiciones de x,y. también llamado Vertex2D), el SetZ (este se encargara del movimiento en el eje z), y tenemos lo que son los GetServo(los cuales servirán para obtener el valor de las variables encapsuladas. con esto termina la programación de la cabecera. se continua con la otra parte de la librería Servo_X.cpp;

#include "mbed.h"
#ifndef SERVO_X_H
#define SERVO_X_H
#define MINPULSE 560
#define MAXPULSE 2400
#define MAXPOS 50// poscisión max de la coordenasda en mm
#define DRAW 50 
#define NODRAW 0


class Servo
{
    private:
        float x,xa,y,ya,z;
        int mm2pulse(float vmm);
    public:
        Servo();
        ~Servo();
        void SetServo(float,float);
        void SetZ(float);
        float GetServoX();
        float GetServoY();
        float GetServoZ();
        void t_espera(float,float);
};

#endif

-Servo_X.cpp

Esta contiene la programación del PWM.

-constructor Servo()

y en esta se introducen el primer movimiento que realizará el PICCOLO CNC, que será ir a la posición de la esquina.

-destructor Servo()

la cual sirve para liberar recursos usados por los objetos.

-mm2pulse

en la cual se podrá introducir un valor vmm (que será la posición en milímetros a la cual se quiere mover).

-SetZ

en la cual se introduce el valor hexadecimal, siendo introducido a mm2pulse obteniendo como resultado el pulso necesario para el movimiento en z.

- SetServo

en el cual se introducen los valores del eje xy respectivamente, y cada uno tendrá el mismo proceso que se realizo en SetZ, en este caso para el eje x y para el eje y.

-GetServoX

obtendrá el valor del eje x.

-GetServoY

obtendrá el valor del eje y.

-GetServoZ

obtendrá el valor del eje z. Y con esto se termina la programación de la librera Servo_X.

#include "mbed.h"
#include "Servo_X.h"
#include "mbed.h"
#include "math.h"
#include "Serial.h"         //serial


PwmOut mypwmX(PA_8);
PwmOut mypwmY(PB_10);
PwmOut mypwmZ(PB_4);

Servo::Servo()
{
    mypwmX.period_ms(20);
    mypwmY.period_ms(20);
    mypwmZ.period_ms(20);
    mypwmX.pulsewidth_us(MAXPULSE);
    mypwmY.pulsewidth_us(MAXPULSE);
    mypwmZ.pulsewidth_us(NODRAW);
    
}

Servo::~Servo()
{
}
void Servo::t_espera(float xi,float yi)
{
    int t=sqrt(xi*xi+yi*yi);
    wait_ms(int((t*100)/60));
}
int Servo::mm2pulse(float vmm)
{
  if (vmm < MAXPOS)
    return int (vmm*(MAXPULSE-MINPULSE)/(MAXPOS)) +MINPULSE;
  return MAXPULSE; 
}
void Servo::SetZ(float _z)
{
    z=_z;
    int PULSEZ=mm2pulse(z);
    mypwmZ.pulsewidth_us(PULSEZ); 
}
void Servo::SetServo(float _x,float _y)
{
    //float xi,yi;
    xa=x;
    ya=y;
    x=_x;
    y=_y;
    //xi=(x-xa)/50;
    //yi=(y-ya)/50;
    //for(int i=1;i<50;i++)
    //{
        //xa=xa+xi;
        //ya=ya+yi;
        int PULSEX=mm2pulse(xa);
        int PULSEY=mm2pulse(ya);
        mypwmX.pulsewidth_us(PULSEX);
        mypwmY.pulsewidth_us(PULSEY);
        //t_espera(xi,yi);
    //}   
}
float Servo::GetServoX()
{
    return x;
}
float Servo::GetServoY()
{
    return y;
}
float Servo::GetServoZ()
{
    return z;
}

Librería para Motor Step

se tiene una librería para el control de los motores paso a paso, la cual permite que desde el programa principal se pueda realizar la manipulación de estos motores, nada mas llamando al objeto y definiendo el atributo que se quiere usar para este objeto, y en el caso de realizar un movimiento se llama al atributo step y se le colocan el numero de pasos que se quiere mover, y en que dirección se quiere mover (adelante 1, atrás 0).

para esta librería tenemos la cabecera donde se define la clase

#ifndef STEP_MOTOR_H
#define STEP_MOTOR_H
 
#include "mbed.h"
 
#define NUME_PASOS 45629
#define RADIO_R 50
 
class stepmotor {
public:
 
    stepmotor(PinName in1, PinName in2, PinName in3, PinName in4,PinName in5, PinName in6, PinName in7, PinName in8); 
    void step(uint32_t num_steps,uint8_t cw);
    void set_speed(int speed);
    uint32_t get_speed();
private:
    BusOut motor_out;
    uint32_t motorSpeed; 
    int8_t nstep;
    int8_t nstep2;
    int8_t ms2;
    void move();
};
 
#endif

stepmotor

comenzara introduciendo los pines para el manejo de ambos motores paso a paso, por lo tanto, los 4 primeros pines es para un motor y los otros 4 son para el otro motor paso a paso, además de ello, aquí se inicializan las siguientes variables.

motor_out=0x0;
    nstep=0;
    motorSpeed=1100;
    nstep2=7;

step

servirá para el movimiento de ambos paso a paso, donde se ingresan el numero de pasos y el sentido en el que lo hará.

pc1.baud(9600);              //programar los baudios
    pc1.format(8,SerialBase::None,1); 
    
    uint32_t count=num_steps ;
    while(count){
        if (cw==1)   nstep++;    
        if (cw==0)  nstep--;  
        if (nstep>7) nstep=0;
        if (nstep<0) nstep=7;
        nstep2=7-nstep;
        move();
        count--;
 
    }
    if(cw==1) pc1.printf("Avance \n");
    else if (cw==0) pc1.printf("Retrocedi \n");

set_speed

servirá para poder modificar la velocidad de los motores paso a paso (1100 es lo mas rápido, entre mas grande el valor, mas lento se moverá)

motorSpeed=speed;

get_speed

sirve para obtener el valor que tiene la velocidad

return motorSpeed;

motor_out

sirve para poder enviar los valores de los motores paso a paso mediante un bus de 8 bits en paralelo. esto permite que ambos motores se puedan mover a la vez

move

sirve para la realización del movimiento de los motores paso a paso, como ambos se mueven la misma magnitud pero sentido contrario, se deben tener dos switch para poder identificar cada paso de ambos motores por separado, pero primero se analizan los pasos para el motor 2, luego en el segundo switch se analizan los pasos del motor 1 pero estos bits se corren 8 bits a la izquierda y se le suman los pasos del anterior switch y este valor va a motor_out

switch(nstep2)
        { 
            case 0: ms2 = 0x1; break;  // 0001
            case 1: ms2 = 0x3; break;  // 0011
            case 2: ms2 = 0x2; break;  // 0010   
            case 3: ms2 = 0x6; break;  // 0110
            case 4: ms2 = 0x4; break;  // 0100
            case 5: ms2 = 0xC; break;  // 1100
            case 6: ms2 = 0x8; break;  // 1000
            case 7: ms2 = 0x9; break;  // 1001
            
            default: ms2 = 0x0; break; // 0000
        }
   
   switch(nstep)
        { 
            case 0: motor_out = 0x10 + ms2; break;  // 0001 XXXX
            case 1: motor_out = 0x30 + ms2; break;  // 0011XXXX
            case 2: motor_out = 0x20 + ms2; break;  // 0010   XXXX
            case 3: motor_out = 0x60 + ms2; break;  // 0110XXXX
            case 4: motor_out = 0x40 + ms2; break;  // 0100XXXX
            case 5: motor_out = 0xC0 + ms2; break;  // 1100XXXX
            case 6: motor_out = 0x80 + ms2; break;  // 1000XXXX
            case 7: motor_out = 0x90 + ms2; break;  // 1001XXXX
            
            default: motor_out = 0x00 + ms2; break; // 0000XXXX
        }
        wait_us(motorSpeed);

Memoria

y tenemos el programa principal, en el cual se programa el puerto serial, se programa la memoria, y en esta memoria se tiene MEM_SIZE el cual tiene el tamaño asignado para la memoria de 10000, tiene el tipo de datos que se le introducirán que es de tipo uint32_t, el apuntador de la cabeza de la memoria, el apuntador de la cola de la memoria, y la variable que nos dirá el estado de la memoria, si esta llena o vacía (o aún con espacio), se programa un arreglo llamado buffer el cual se le introducirá el tamaño que tendrá este arreglo, que en nuestro caso se le introduce MEM_SIZE, se tiene el liberador de memoria, el cual dejara el apuntador de la cabeza, la cola y la variable full en 0. también se tiene la función mem_put la cual se encargara de escribir en la memoria, primero mirando si la memoria no se encuentra llena, en el caso que sí, esta retornara un 1, en el caso que todavía tenga espacio, se toma el arreglo buffer y en la posición de la cabeza, se le guarda el dato, y la posición de la cabeza aumenta una posición, y en el caso que al realizar este procedimiento, la memoria se lleno o no, y en el caso que si, la variable full quedara cargada con un 1 y luego de esto retornara un 0 el cual indicará que el guardado se realizo de forma correcta. también se tiene la función mem_get, la cual estará encargada de obtener los valores que están almacenados en la memoria, y comienza preguntando sí la cabeza esta en 0 (esto permite saber si la memoria tiene algo guardado y si no, entonces retorna un 1 que indicara que la memoria esta vacía, luego realiza otra pregunta en el caso que la cabeza sea igual que la cola, ya que de ser así es por que se termino de leer toda la memoria. sí ninguna de las condiciones anteriores no se cumple, procederá a obtener el dato que se encuentre en la cola, luego aumentando una posición en la cola y retornando un 0 que permitirá saber si el procedimiento se realizo bien.

 #define MEM_SIZE 10000
#define MEM_TYPE uint32_t
 
uint32_t mem_head = 0;
uint32_t mem_tail = 0;
uint32_t full = 0;
 
MEM_TYPE buffer[MEM_SIZE];
 
void mem_free()
{
    mem_head=0;
    full=0;
    mem_tail=0;   
}
 
uint32_t mem_put(MEM_TYPE data)
{
    if(full){
        return 1;
    }
    buffer [mem_head] = data;
    mem_head +=1;
    if(mem_head == MEM_SIZE)
    full=1;
    return 0;
}
 
uint32_t mem_get(MEM_TYPE* data)
{
    if(mem_head == 0)
        return 1;
    if (mem_head == mem_tail)
        return 1;
        
    *data=buffer[mem_tail];
    mem_tail +=1;
    
    return 0;
}

Comunicación Serial

también tenemos la función program_serial() el cual estará encargado de la programación de los protocolos para la realización de la comunicación serial.

pc.baud(9600);              //programar los baudios
    pc.format(8,SerialBase::None,1);        //el numero de datos en bits, el bit de paridad, el bit de stop

Funciones

la función ejecutar()

la cual esta encargada de realizar el movimiento de todas las posiciones que anteriormente fueron guardadas.

la función guardar()

que se encargara de guardar los comandos que se envían desde el computador, estos comandos son;

-dibujar 0xfc

-no dibujar 0xfb

-servo 0xfd

(este comando permite saber que los dos siguientes comandos, son las posiciones xy).

-Step_Motor 0xf9

(este comando permite saber que los dos siguientes comandos, son el numero del cuadrante y la dirección (si es 1 es adelante y si es 0 es atras).

- end 0xf0

(permite tener la certeza que la comunicación se esta realizando bien).

- stop 0xfa

(permite saber cuando se dejaran de enviar comandos). siguiente al guardado, se tiene el programa principal, el cual comenzara programando los protocolos de la comunicación serial a través de program_serial, programando variables de tipo uint8_t llamadas letra y subletra, programando el objeto llamado Servos que pertenecerá a la clase Servo, luego de esto entrará a un ciclo infinito donde se enviara por comunicación "ingrese comando" para que el usuario ingrese el comando deseado ya sea de ejecutar (0xff) o guardar (0xfe), ya con esto a través de un Switch se escoge que comando fue el recibido, y en el caso de no ser un comando anteriormente mencionado, hará que el usuario vuelva a digitar nuevamente el comando. con esto se realiza la programación de la función program_serial(). la programación de ejecutar() donde se volverá a programar el objeto la comunicación y esta tendrá 4 variables de tipo entero y de 8 bits llamadas a,b,c,d, y 4 variables enteras de 32 bits, y se ingresara a la lectura de la memoria, y el valor obtenido será de 32 bits y se guardaran en las 4 variables de 32 bits que ya habíamos programado, donde estas serán operadas con un AND ya sea de 0xff, 0xff00, 0xff0000 ó 0xff000000 esto con el fin de obtener el valor de cada comando guardado, y siguiente a esto se correrán los bits necesarios para que queden en los 8 primeros bits, y sean asignados a las variables de 8bits que ya habíamos asignado. teniendo esto se sabe, cual que hará y a que posiciones lo hará, dependiendo de los comandos que se hayan guardado con anterioridad. la programación de guardar(), comienza con un liberado de memoria, se programa una variable de 32 bits llamada letra y otra variable de 8 bits llamada subletra, donde primero se obtiene el valor del primer comando y se le asigna a subletra, esta permitirá saber a que índole pertenece ya sea servo, dibujar o no dibujar, en el caso de entrar en servos, a letra se le será asignado el valor de la subletra ya desplazado 8bits, y se vuelve a obtener el comando de la comunicación, y se almacena a subletra, donde a letra se le asignara la suma de ella misma y de la subletra donde luego a letra se le desplazaran 8bits, y con esto se obtiene un espacio de memoria para cada línea de código enviado desde el computador, lo mismo pasa para el caso de dibujar y no dibujar pero en este caso nada mas se desplazara 8bits.

Interrupción

esta interrupción permite el pausado y despausado del sistema al estar en movimiento, a través de los comando f1,f2, siendo cada uno para pausar y despausar respectivamente.

void Pausa()
{
        int b=pc.getc();
        if(b==INTER){
        pc.attach(0,Serial::RxIrq);
        pc.printf("me pause \n");
        while(pc.getc()!=RUN){}
        pc.printf("me despause \n");
        pc.attach(&Pausa,Serial::RxIrq);
        }
        return;
}

Con esto se logra que el programa al recibir algo por el puerto serial, se detenga y atienda a la interrupción, por lo tanto sí es el comando de pausa, el programa lo detecta y pausa al sistema dejándolo en un while hasta que llegue el comando de despausar, además de esto se debe deshabilitar la interrupción con el fin de que al llegar el comando de despause, no vuelva a activar la interrupción.

Aplicación

La aplicación se desarrollo en App Inventor, esto para poder comunicar la tarjeta y el celular, y poder aprovechar los sensores del celular.

La programación se hace a través de bloques, siendo una programación para niños, por lo tanto sencilla, pero muy útil.

se le agregaron nuevas funciones