9. ANEXOS
9.3. ANEXO C: Código de control del motor PAP
#include <SoftwareSerial.h> SoftwareSerial BT (3, 4); //RX, TX /* Variables Driver Paso a Paso */ const int enable = 5;
const int btn_avanza = 6; const int btn_retrocede = 7; const int dirPin = 8; const int stepPin = 9; const int MODE_0 = 10; const int MODE_1 = 11; const int MODE_2 = 12; const int control_led = 13;
const int STOP_pin = 19; // uso el A5 como salida digital float steps = 1; // se configura con el seteo del volumen unsigned long steps_prev;
unsigned long stepDelay = 25; // se configura con el seteo del flujo unsigned long stepDelay_prev;
unsigned long stepDelay_entero; unsigned int stepDelay_decimal; int jeringa;
int flujo; int volumen;
//unsigned long contador = 0; //para developers, cuenta el nro de pasos. int micro_step = 32;
bool flag_manual = 0;//inicializa en manual ya que se ejecuta la funcion al principio del programa bool flag_STOP = 0;
/* Variables Driver Paso a Paso */ void setup() {
BT.begin (115200); Serial.begin (115200); /* Pines Driver Paso a Paso */
pinMode (enable, OUTPUT); // activar o desacivar Salida al motor digitalWrite (enable, HIGH); //HIGH : Salidas desactivadas al inicio pinMode(dirPin, OUTPUT);
pinMode(stepPin, OUTPUT); pinMode(btn_avanza, INPUT); pinMode(btn_retrocede, INPUT);
pinMode(MODE_0, OUTPUT); // microsteping MODE 0 pinMode(MODE_1, OUTPUT); // microsteping MODE 1 pinMode(MODE_2, OUTPUT); // microsteping MODE 2
pinMode(control_led, OUTPUT); //LED DE CONTROL (para confiracion de envio y recepcion de datos) funcion "blink_led()"
pinMode (STOP_pin, OUTPUT); digitalWrite (STOP_pin, LOW);
digitalWrite (MODE_0, HIGH); // configuracion a 1/16 1;1;1 .... POLOLU a4988 digitalWrite (MODE_1, HIGH);
digitalWrite (MODE_2, HIGH); digitalWrite (control_led, LOW);
attachInterrupt( 0, Emergencia, RISING); //linea de interrupcion de emergencia /* Pines Driver Paso a Paso */
control_ManAuto(); //inicia en modo manual }
void loop() {
if (flag_STOP == 1){ //reviso si el boton de emergencia fue presionado STOP();
if (BT.available ()) { String bufferString = "";
while (BT.available() > 0) { //construyo la palabra completa bufferString += char (BT.read());
}
Serial.println (bufferString); //no puedo usar un switch porque es un string la variable if (bufferString == "JER") {
blink_led (2); cambiar_jeringa (); }
else if (bufferString == "VOL") { blink_led (2);
cambiar_vol (); }
else if (bufferString == "FLUJO") { blink_led (2);
cambiar_flujo (); }
else if (bufferString == "MA") { // cambio entre mando manual y automatico. control_ManAuto();
}
else if (bufferString == ">") { avanzar ();
}
else if (bufferString == "<") { //boton seteado en el maestro retroceder();
} else { blink_led (3);
BT.println ('0'); //mando un cero para avisar que hay un error de comando Serial.println ("error de comando");
} }
/*Logica del Serial*/ /*Logica de los Pulsadores*/
if (digitalRead (btn_avanza) == HIGH && digitalRead (btn_retrocede) == LOW) { avanzar ();
}
else if (digitalRead (btn_avanza) == LOW && digitalRead (btn_retrocede) == HIGH) { retroceder();
}
else if (digitalRead (btn_avanza) == HIGH && digitalRead (btn_retrocede) == HIGH) { blink_led(3); //blinkeo 3 veces el led
delay (500); }
/*Logica de los Pulsadores*/ }
void control_ManAuto (){ //debo guardar los valores previamente configurados, cambiar para velocidad rapida y luego recuperar los valores previos.
flag_manual =! flag_manual; if (flag_manual == 1){ steps_prev = steps;
stepDelay_prev = stepDelay; steps = 50;
stepDelay = 25; //maximo premitido para que el motor no salte pasos)
stepDelay_entero = int(stepDelay/1000); //Obtengo solo la parte entera en mS. Esto es lo que se demora la funcion delay
stepDelay_decimal = stepDelay - (stepDelay_entero*1000); //obtengo la parte decimal y la paso a uS. Esto es lo que se demora la funcion delayMicrosecond (MAX: 16383)
blink_led (2);
Serial.println ("Control Manual"); }
else {
steps = steps_prev;
stepDelay = stepDelay_prev;
stepDelay_entero = int(stepDelay/1000); //Obtengo solo la parte entera en mS. Esto es lo que se demora la funcion delay
stepDelay_decimal = stepDelay - (stepDelay_entero*1000); //obtengo la parte decimal y la paso a uS. Esto es lo que se demora la funcion delayMicrosecond (MAX: 16383)
blink_led (2);
BT.print ('A'); //envio una A para avisar que esta en modo automatico Serial.println ("Control Automatico");
} }
void cambiar_jeringa () {
if (flag_STOP == 1){ //reviso si el boton de emergencia fue presionado STOP();
}
Serial.print ("Jeringa: "); while (BT.available () == 0) { } //espero a recibir un valor por serial if (BT.available ()) {
char aux = BT.read(); delay(10);
Serial.println(aux);
blink_led(2); //confirmacion de recepcion de dato if (aux == '1') {
jeringa = 1;
BT.print ('1');// jeringa configurada correctamente BT.flush(); } else if (aux == '2') { jeringa = 2; BT.print ('1'); BT.flush(); } else if (aux == '3') { jeringa = 3; BT.print ('1'); BT.flush(); } else {
BT.print ('0'); //envio un cero para avisar que hubo un error BT.flush(); cambiar_jeringa(); } } cambiar_flujo (); cambiar_vol (); }
void cambiar_flujo () { //se ingresa en ul/min
if (flag_STOP == 1){ //reviso si el boton de emergencia fue presionado STOP();
}
Serial.print ("Flujo: "); while (BT.available () == 0) { } //espero a recibir un valor por serial if (BT.available ()) {
String bufferString_flujo = "";
while (BT.available() > 0) { //construyo la palabra completa bufferString_flujo += char (BT.read());
flujo = bufferString_flujo.toInt(); //convierto a INT el string Serial.println (flujo);//borrar dps
blink_led (2); //Led 13 parpadea 2 veces para indicar OK }
cambiar_vel(); // una vez seteado el flujo debo cambiar la velocidad del paso dependiendo de la jeringa seleccionada.
//Serial.print ("dato enviado ");
//BT.flush(); //me aseguro de limpiar el BUS de envio }
void cambiar_vel () {
Serial.print ("Step delay [us]: "); switch (jeringa) {
case (1):
stepDelay = (float)(7E-2 / (flujo * micro_step)) * 60E6; //tiempo_por_paso [us]=
(volumen_por_paso/flujo_deseado*micropasos_por_paso)*60000000 ; si 2000 pasos son 0,14mL....1 paso son 7*10^-5 mL = 7*10^-2 ul
Serial.println (stepDelay); if (stepDelay < 25) {
BT.print ('0'); //envío un 0 para comunicar que el dato ingresado no es valido cambiar_flujo();
} else {
BT.print ('1'); //con comillas simples ya que es UN solo caracter ; envio un 1 para comunicar que el valor seteado es correcto
BT.flush(); }
break; case (2):
stepDelay = (0.5 / (flujo * micro_step)) * 60E6; //si 2000 pasos son 1mL....1 paso son 5*10^-4 mL Serial.println (stepDelay); if (stepDelay < 25) { BT.println ('0'); cambiar_flujo(); } else { BT.println ('1'); //BT.flush(); } break; case (3):
stepDelay = (0.7175 /(flujo * micro_step)) * 60E6; //si 2787.5 pasos son 2mL....1 paso son 7.175*10^-4 mL Serial.println (stepDelay); if (stepDelay < 25) { BT.println ('0'); cambiar_flujo(); } else { BT.println ('1'); BT.flush(); } break; }
/* calculo exacto del delay para solventar problemas de decimales no realizados en delay y delayMicrosecconds*/ stepDelay_entero = int(stepDelay/1000); //Obtengo solo la parte entera en mS. Esto es lo que se demora la funcion delay
stepDelay_decimal = stepDelay - (stepDelay_entero*1000); //obtengo la parte decimal y la paso a uS. Esto es lo que se demora la funcion delayMicrosecond (MAX: 16383)
Serial.println (stepDelay_entero); Serial.println (stepDelay_decimal);
/* calculo exacto del delay para solventar problemas de decimales no realizados en delay y delayMicrosecconds*/ }
if (flag_STOP == 1){ //reviso si el boton de emergencia fue presionado STOP();
}
Serial.print ("Volumen: "); while (BT.available () == 0) { } //espero a recibir un valor por serial if (BT.available ()) {
String bufferString_vol = "";
while (BT.available() > 0) { //construyo la palabra completa bufferString_vol += (char)BT.read();
}
volumen = bufferString_vol.toInt(); }
blink_led(2); //dato recibido correctamente
cambiar_steps (); //debo setear la cantidad de pasos a realizar en funcion del volumen ingresado en ul y la jeringa seleccionada
Serial.println (volumen);
BT.println ('1'); //envio un dato de OK BT.flush();
}
void cambiar_steps () { /*
DATOS IMPORTANTES: Para un motor NEMA 17 de 1,8° por paso y una varilla roscada de 0,8mm de paso X 5mm de diametro, 1 paso full step = 4 micrómetros => 2000 pasos = 8 mm */
float volumen_aux = volumen * 1E-3; //por cuestiones tecnicas de arduino hizo falta definir esta variable. switch (jeringa) {
case (1):
steps = (float)(volumen_aux * micro_step * 2000)/140; // Cálculo realizado para jeringa estandar de 1ml (depende del diametro del émbolo) y los "DATOS IMPORTANTES"
steps = steps * 1E3; //Serial.println (steps); break;
case (2):
steps = (float)(volumen_aux * micro_step * 2000) / 1000; // Cálculo realizado para jeringa estandar de 5ml steps = steps * 1E3;
break; case (3):
steps = (float) volumen * micro_step; // Cálculo realizado para jeringa estandar de 10ml steps = steps * 1E3;
break; } }
void avanzar () {
digitalWrite (enable, LOW);//Activo las salidas del driver
digitalWrite(dirPin, HIGH);//set de la dirección de giro, OJO depende de la conexion del motor al driver //unsigned long t0 = millis(); //para registrar el tiempo que tarda en avanzar los "n" pasos
for (long x = 0; x < steps; x++) { digitalWrite(stepPin, HIGH); delay(stepDelay_entero);
delayMicroseconds(stepDelay_decimal); digitalWrite(stepPin, LOW);
}
//Serial.println (millis() - t0); // descomentar para imprimir el tiempo total por por bolo digitalWrite (enable, HIGH);//Desactivo las salidas del driver
}
void retroceder () {
digitalWrite (enable, LOW);//Activo las salidas del driver
digitalWrite(stepPin, HIGH); delay(stepDelay_entero);
delayMicroseconds(stepDelay_decimal); digitalWrite(stepPin, LOW);
}
digitalWrite (enable, HIGH); //Desactivo las salidas del driver }
void Emergencia () {
digitalWrite (enable, HIGH);//Desactivo las salidas del driver
steps = 0; //finalizo cualquier avance o retroceso que haya estado en proceso flag_STOP = 1;
}
void STOP (){
digitalWrite (STOP_pin, HIGH); //reseteo el otro arduino y espero confirmacaion de él delay(50);
digitalWrite (STOP_pin, LOW); Serial.println ("STOP"); while (BT.available () == 0) {
blink_led (1); //parpadea el led hasta recibir la orden de emergencia solucionada } //espero a recibir un valor por serial para salir de la emergencia
if (BT.available ()) { char aux = BT.read();
if (aux == 'R'){ //señal de emergencia solucionada. Reseteo el arduino. flag_STOP = 0;
asm volatile ("jmp 0"); //reseteo el arduino }
} }
void blink_led (int n){
delay (500); //espero medio segundo para efectuar otro ciclo si fue enviado for (int i=0; i<n; i++){
digitalWrite (control_led, HIGH); delay (250);
digitalWrite (control_led, LOW); delay (50);
} }