Dispositivo de entrenamiento de movilidad motorizados para niños pequeños (7 / 14 paso)

Paso 7: Electrónica: subir código

Añadir el siguiente código a la placa Arduino Uno. Puede encontrar instrucciones sobre cómo configurar su arduino aquí: https://www.arduino.cc/en/Guide/HomePage

comienzo del código

const int DIR_A = 2, pin de escudo A DIR //Motor

DIR_B = 4, pin de escudo B DIR //Motor

PWM_A = 3, pin de escudo A PWM //Motor

PWM_B = 5, pin de escudo A PWM //Motor

POT = A3, pin //Potentiometer

ADELANTE = 8, pin //Forward

REVERSO = 9, //Reverse pin

IZQUIERDA = 11, //Left pin

DERECHA = 10, pin //Right

CURRENT_A = A1, pin del Sensor A de //Current

CURRENT_B = A0; Pin actual Sensor B

int DIR, OLD_DIR; Controlador de entrada y entrada de historia

Boolean stop_all = false; Disminuir la velocidad a 0

Declarar variables

int MAX_SPEED = 230, MIN_SPEED = 60;

acc de flotador = 0;

Float LEFT_SPEED = 0;

Float RIGHT_SPEED = 0;

Float COMP_A = 0;

Float COMP_B = 0;

int LEFT_MOTOR_DIR = 0;

int RIGHT_MOTOR_DIR = 0;

doble TIME_TO_MAX_SPEED = 5000;

int retardo = 100;

void setup() {}

Definición de pines de entrada y salida

pinMode (DIR_A, salida);

pinMode (DIR_B, salida);

pinMode (PWM_A, salida);

pinMode (PWM_B, salida);

pinMode (derecha, INPUT_PULLUP);

pinMode (izquierda, INPUT_PULLUP);

pinMode (delantero, INPUT_PULLUP);

pinMode (reversa, INPUT_PULLUP);

pinMode (bote, entrada);

pinMode (CURRENT_A, entrada);

pinMode (CURRENT_B, entrada);

Inicializa el puerto serial para monitoreo si es necesario

Serial.Begin(9600);

DIR = (digitalRead(FORWARD) << 3)

+ (digitalRead(REVERSE) << 2)

+ (digitalRead(LEFT) << 1)

+ (digitalRead(RIGHT));

OLD_DIR = DIR;

Disminuir la frecuencia PWM para reducir el ruido del motor

setPwmFrequency(3,1);

setPwmFrequency(5,1);

}

void loop() {}

Lee la entrada del regulador

ADELANTE: B0111

REVERSO: B1011

IZQUIERDA: B1101

DERECHA: B1110

DIR = (digitalRead(FORWARD) << 3)

+ (digitalRead(REVERSE) << 2)

+ (digitalRead(LEFT) << 1)

+ (digitalRead(RIGHT));

LEE EL POTENCIÓMETRO Y MAPAS AL MÍNIMO Y VELOCIDAD MÁXIMA

int velocidad = map(analogRead(POT), 0, 1023, MIN_SPEED, MAX_SPEED);

LEE EL SENSOR DE CORRIENTE PARA EQUILIBRAR LA CARGA EN AMBOS MOTORES

I_A doble = (((double)analogRead(CURRENT_A)) * 5/1023.0) - 2,5) / 0,1;

I_B doble = (((double)analogRead(CURRENT_B)) * 5/1023.0) - 2,5) / 0,1;

SI NO PRESIONA NINGÚN BOTÓN, PARAR LOS MOTORES

Si (DIR == B1111) {}

stop_all = true;

}

Si (stop_all) {}

¿Left_speed = (LEFT_SPEED < = 0)? 0: LEFT_SPEED-(SPEED/TIME_TO_MAX_SPEED*DELAY*2);

¿Right_speed = (RIGHT_SPEED < = 0)? 0: RIGHT_SPEED-(SPEED/TIME_TO_MAX_SPEED*DELAY*2);

}

Si (LEFT_SPEED == 0 & & RIGHT_SPEED == 0 & & B1111) {}

stop_all = false;

OLD_DIR = DIR;

}

SI LOS MOTORES SON INACTIVOS, ESCUCHAR PARA LA ENTRADA DE CONTROLADOR

Si (stop_all == false) {}

Si (DIR! = OLD_DIR & & digitalRead(FORWARD)! = B0) {}

DIR = OLD_DIR;

}

{Switch(dir)}

B0111 del caso: //FORWARD

compensación de sensor actual

Si {} (abs(I_A-I_B) > 0.1)

Si (I_A > I_B) {}

COMP_A = (I_A-I_B) * VELOCIDAD/TIME_TO_MAX_SPEED * RETRASO * 0.1;

COMP_B = (I_A-I_B) *-VELOCIDAD/TIME_TO_MAX_SPEED * RETRASO * 0.1;

}

Else {}

COMP_A = (I_A-I_B) *-VELOCIDAD/TIME_TO_MAX_SPEED * RETRASO * 0.1;

COMP_B = (I_A-I_B) * VELOCIDAD/TIME_TO_MAX_SPEED * RETRASO * 0.1;

}

}

Else {}

COMP_A = 0;

COMP_B = 0;

}

calcular la aceleración

ACC = (velocidad-LEFT_SPEED) * 0.1;

¿ACC = (acc > = 6)? 6: acc;

¿ACC = (acc < = 1)? 1: acc;

Añadir aceleración y compensación a los motores izquierdos y derecho

LEFT_SPEED = LEFT_SPEED + acc + COMP_B;

RIGHT_SPEED = RIGHT_SPEED + acc + COMP_A;

¿Left_speed = (LEFT_SPEED > = velocidad)? VELOCIDAD: LEFT_SPEED;

¿Right_speed = (RIGHT_SPEED > = velocidad)? VELOCIDAD: RIGHT_SPEED;

Establecer direcciones de motor, 1, = -1 = marcha atrás

LEFT_MOTOR_DIR = 1;

RIGHT_MOTOR_DIR = 1;

rotura;

B0101 del caso: //steer a la izquierda cuando se mueve hacia adelante

LEFT_SPEED = LEFT_SPEED * 0,9;

RIGHT_SPEED = RIGHT_SPEED;

rotura;

caso B0110: //steer derecha cuando avanza

LEFT_SPEED = LEFT_SPEED;

RIGHT_SPEED = RIGHT_SPEED * 0,9;

rotura;

B1011 del caso: //REVERSE

compensación de sensor actual

Si {} (abs(I_A-I_B) > 0.1)

Si (I_A > I_B) {}

COMP_A = (I_A-I_B) * VELOCIDAD/TIME_TO_MAX_SPEED * RETRASO * 0.1;

COMP_B = (I_A-I_B) *-VELOCIDAD/TIME_TO_MAX_SPEED * RETRASO * 0.1;

}

Else {}

COMP_A = (I_A-I_B) *-VELOCIDAD/TIME_TO_MAX_SPEED * RETRASO * 0.1;

COMP_B = (I_A-I_B) * VELOCIDAD/TIME_TO_MAX_SPEED * RETRASO * 0.1;

}

}

Else {}

COMP_A = 0;

COMP_B = 0;

}

calcular la aceleración

ACC = (velocidad-LEFT_SPEED) * 0.1;

¿ACC = (acc > = 6)? 6: acc;

¿ACC = (acc < = 1)? 1: acc;

Añadir aceleración y compensación a los motores izquierdos y derecho

LEFT_SPEED = LEFT_SPEED + acc + COMP_B;

RIGHT_SPEED = RIGHT_SPEED + acc + COMP_A;

¿Left_speed = (LEFT_SPEED > = velocidad)? VELOCIDAD: LEFT_SPEED;

¿Right_speed = (RIGHT_SPEED > = velocidad)? VELOCIDAD: RIGHT_SPEED;

Establecer direcciones de motor, 1, = -1 = marcha atrás

LEFT_MOTOR_DIR = -1;

RIGHT_MOTOR_DIR = -1;

rotura;

B1101 del caso: //LEFT

calcular la aceleración

ACC = (velocidad-LEFT_SPEED) * 0.1;

¿ACC = (acc > = 15)? 8: acc;

¿ACC = (acc < = 1)? 1: acc;

Añadir aceleración y compensación a los motores izquierdos y derecho

LEFT_SPEED = LEFT_SPEED + acc;

RIGHT_SPEED = RIGHT_SPEED + acc;

¿Left_speed = (LEFT_SPEED > = velocidad)? VELOCIDAD: LEFT_SPEED;

¿Right_speed = (RIGHT_SPEED > = velocidad)? VELOCIDAD: RIGHT_SPEED;

Establecer direcciones de motor, 1, = -1 = marcha atrás

LEFT_MOTOR_DIR = -1;

RIGHT_MOTOR_DIR = 1;

rotura;

B1110 del caso: //RIGHT

calcular la aceleración

ACC = (velocidad-LEFT_SPEED) * 0.1;

¿ACC = (acc > = 15)? 8: acc;

¿ACC = (acc < = 1)? 1: acc;

Añadir aceleración y compensación a los motores izquierdos y derecho

LEFT_SPEED = LEFT_SPEED + acc;

RIGHT_SPEED = RIGHT_SPEED + acc;

¿Left_speed = (LEFT_SPEED > = velocidad)? VELOCIDAD: LEFT_SPEED;

¿Right_speed = (RIGHT_SPEED > = velocidad)? VELOCIDAD: RIGHT_SPEED;

Establecer direcciones de motor, 1, = -1 = marcha atrás

LEFT_MOTOR_DIR = 1;

RIGHT_MOTOR_DIR = -1;

rotura;

}

}

instrucciones motor ajustadas

Si (RIGHT_MOTOR_DIR > 0) {}

digitalWrite (DIR_A, LOW);

}

Else {}

digitalWrite (DIR_A, alto);

}

Si (LEFT_MOTOR_DIR > 0) {}

digitalWrite (DIR_B, bajo);

}

Else {}

digitalWrite (DIR_B, alto);

}

ajustar la velocidad del motor

¿Left_speed = LEFT_SPEED < = 0? 0: LEFT_SPEED;

¿Right_speed = RIGHT_SPEED < = 0? 0: RIGHT_SPEED;

analogWrite (PWM_A, RIGHT_SPEED);

analogWrite (PWM_B, LEFT_SPEED);

OLD_DIR = DIR;

Serial.Print(Speed);

Serial.Print("|");

Serial.Print("Left:");

Serial.Print(LEFT_SPEED);

Serial.Print("|");

Serial.Print("Right:");

Serial.println(RIGHT_SPEED);

Delay(Delay);

}

Ajusta la frecuencia del PWM

{} void setPwmFrequency (int pin, int divisor)

modo de octeto;

Si (pin == 5 || pin == 6 || pin == 9 || pin == 10) {}

{Switch(divisor)}

caso 1:

modo = 0 x 01;

rotura;

caso 8:

modo = 0 x 02;

rotura;

caso 64:

modo = 0 x 03;

rotura;

caso 256:

modo = 0 x 04;

rotura;

caso 1024:

modo = 0 x 05;

rotura;

por defecto:

retorno;

}

Si (pin == 5 || pin == 6) {}

TCCR0B = TCCR0B & 0b11111000 | modo de;

}

Else {}

TCCR1B = TCCR1B & 0b11111000 | modo de;

}

}

else if (pin == 3 || pin == 11) {}

{Switch(divisor)}

caso 1:

modo = 0 x 01;

rotura;

caso 8:

modo = 0 x 02;

rotura;

caso 32:

modo = 0 x 03;

rotura;

caso 64:

modo = 0 x 04;

rotura;

caso 128:

modo = 0 x 05;

rotura;

caso 256:

modo = 0 x 06;

rotura;

caso 1024:

modo = 0x7;

rotura;

por defecto:

retorno;

}

TCCR2B = TCCR2B & 0b11111000 | modo de;

}

}

final del código de