Paso 5: autoguiado hacia el blanco
Incluso cuando intentamos nuestro mejor para contar cada pulso puede haber ocasiones en podemos perder unos cuantos y poco a poco pierden precisión con el tiempo. Para minimizar esto que podemos utilizar una rutina autoguiados hacia el blanco! La idea es enviar el actuador a una determinada posición (totalmente extendido o completamente retraída) y las cuentas en un valor conocido. Generalmente es más fácil retraer el actuador y las cuentas se establece en 0. En el código siguiente que se restablecerá hasta cuenta valor cuando completamente extendido y restablecer a 0 cuando está totalmente retraído.
Para ello necesita una manera de saber cuando el motor está en sus límites. Aquí vamos a utilizar la detección de la corriente de la MegaMoto a ver cuando la corriente cae a 0. Cuando vemos que el actuador tiene golpear el interruptor de límite y detenido. Tenemos un pequeño contador corriendo ya que a veces la corriente puede denunciar a un falso 0. Asegurándose de que la corriente es 0 para una longitud del tiempo que sabemos que el motor realmente está en un límite y no obtener lecturas falsas.
Ver código adjunto y los comentarios para más información:
#define amp0 A5 #define PWMA0 6 #define PWMB0 5 #define enable0 13 //pins for first MegaMoto #define switch0 7 //Up button to add counts #define switch1 8 //Down button to subtract counts #define hall0 2 //interrupt pins for hall effect sensors int enable = 0; //enable pin for megaMoto int lowampLimit = 0;//Low limit to detect when actuator stops int amps = 0; //current readings int timedelay[] = {750, 50}; //first, regular delay for feedback int hitLimits = 0; int hitLimitsmax = 10;//value for knowing when retracted int count[] = {0};//Actuator int maxCounts = 1150;//number of counts when fully extended int sw[] = {1, 1}; //switch up, switch down int prev[] = {0, 0};//previous switch state int currentPos = 0;//current position int threshold = 1; int destination = 0; bool forwards = false; bool backwards = false;// motor states bool extended = false; bool retracted = false;//actuator positions bool firstRun = true;//first run of the motor once the button is pushed void setup() { pinMode(amp0, INPUT); digitalWrite(amp0, LOW);//set Current sensors pinMode(PWMA0, OUTPUT); pinMode(PWMB0, OUTPUT);//set PWM outputs pinMode(enable0, OUTPUT); digitalWrite(enable0, LOW);//set enable and turn board OFF pinMode(switch0, INPUT); pinMode(switch1, INPUT); digitalWrite(switch0, HIGH); digitalWrite(switch1, HIGH);//set up/down switch, enable enternal relays pinMode(hall0, INPUT); digitalWrite(hall0, LOW);//set hall, set low to start for rising edge attachInterrupt(0, speed0, RISING); //enable the hall effect interupts retracted = true;//start retracted extended = false; Serial.begin(9600); }//end setup void loop() { ReadInputs();//check input button, calculate speeds if (sw[0] == 0 && sw[1] == 1 && backwards == false) destination = currentPos - 115;//dont change destination while moving else if (sw[0] == 1 && sw[1] == 0 && forwards == false) destination = currentPos + 115;//dont change destination while moving Serial.print("count[0] "); Serial.println(count[0]); Serial.print("currentPos "); Serial.println(currentPos); Serial.print("destination "); Serial.println(destination); if ((destination >= (currentPos - threshold)) && (destination <= (currentPos + threshold))) stopMoving();//stop if you're close enough else if (destination > currentPos) goForwards();//move if you need to else if (destination < currentPos) goBackwards();//move if you need to for (int i = 0; i <= 1; i++) prev[i] = sw[i]; //store switch values as previous values }//end loop void speed0() { if (forwards == true) count[0]++; //if moving forwards, add counts else if (backwards == true) count[0]--; //if moving back, subtract counts }//end speed0 void ReadInputs() { amps = analogRead(amp0);//read current sw[0] = digitalRead(switch0), sw[1] = digitalRead(switch1);//check switches currentPos = count[0];//set where you are }//end read inputs void goForwards() { forwards = true; backwards = false;//set travel direction getFeedback();//check current draw digitalWrite(enable0, HIGH);//enable board analogWrite(PWMA0, 255); analogWrite(PWMB0, 0);//apply speeds }//end goForwards void goBackwards() { forwards = false; backwards = true;//set travel direction getFeedback();//check current draw digitalWrite(enable0, HIGH);//enable board analogWrite(PWMA0, 0); analogWrite(PWMB0, 255);//apply speeds }//end goBackwards void stopMoving() { forwards = false; backwards = false;//set travel direction analogWrite(PWMA0, 0); analogWrite(PWMB0, 0);//set speeds to 0 delay(10); digitalWrite(enable0, LOW);//disable board }//end stopMoving void getFeedback() { amps = analogRead(amp0); Serial.print(" Amp readings - "), Serial.println(amps); if (amps <= lowampLimit && hitLimits < hitLimitsmax) hitLimits = hitLimits + 1; else hitLimits = 0; if (hitLimits == hitLimitsmax && backwards == true) { Serial.println("RETRACTED"); retracted = true; count[0] = 0; //reset counter when homed destination = 0; } if (hitLimits == hitLimitsmax && forwards == true) { Serial.println("EXTENDED"); extended = true; count[0] = maxCounts; //reset counter when extended destination = maxCounts; } }//end getFeedback
