Motordriven rörlighet bildning anordning för småbarn (7 / 13 steg)

Steg 7: Elektronik: ladda upp koden

Ladda upp följande kod till Arduino Uno styrelsen. Du kan hitta instruktioner om hur du ställer in din arduino här:https://www.arduino.cc/en/Guide/HomePage

början av kod
CONST int DIR_A = 2, //Motor sköld A DIR pin
DIR_B = 4, //Motor sköld B DIR pin
PWM_A = 3, //Motor sköld en PWM stift
PWM_B = 5, //Motor sköld en PWM stift
KRUKA = A3, //Potentiometer pin
FRAMÅT = 8, //Forward pin
OMVÄND = 9, //Reverse pin
VÄNSTER = 11, //Left pin
HÖGER = 10, //Right pin
CURRENT_A = A1, //Current Sensor en pin
CURRENT_B = A0; Nuvarande Sensor B pin
int DIR, OLD_DIR; Controller-ingång och Input historia
booleska stop_all = false; Minska hastigheten 0
Deklarera variabler
int MAX_SPEED = 230, MIN_SPEED = 60.
flyta acc = 0;
flyta LEFT_SPEED = 0;
flyta RIGHT_SPEED = 0;
flyta COMP_A = 0;
flyta COMP_B = 0;
int LEFT_MOTOR_DIR = 0;
int RIGHT_MOTOR_DIR = 0;
Double TIME_TO_MAX_SPEED = 5000;
int dröjsmål = 100;
void setup() {
Definiera indata och utdata pins
pinMode (DIR_A, OUTPUT);
pinMode (DIR_B, OUTPUT);
pinMode (PWM_A, OUTPUT);
pinMode (PWM_B, OUTPUT);
pinMode (rätt, INPUT_PULLUP);
pinMode (vänster, INPUT_PULLUP);
pinMode (FORWARD, INPUT_PULLUP);
pinMode (bakåt, INPUT_PULLUP);
pinMode (KRUKA, indata);
pinMode (CURRENT_A, indata);
pinMode (CURRENT_B, indata);
Initierar seriell port för övervakning vid behov
Serial.BEGIN(9600);
DIR = (digitalRead(FORWARD) << 3)
+ (digitalRead(REVERSE) << 2)
+ (digitalRead(LEFT) << 1)
+ (digitalRead(RIGHT));
OLD_DIR = DIR;
Minska PWM frekvens för att minska motor gnälla
setPwmFrequency(3,1);
setPwmFrequency(5,1);
}
void loop() {
Läser den registeransvarige input
FRAM: B0111
OMVÄND: B1011
VÄNSTER: B1101
HÖGER: B1110
DIR = (digitalRead(FORWARD) << 3)
+ (digitalRead(REVERSE) << 2)
+ (digitalRead(LEFT) << 1)
+ (digitalRead(RIGHT));
LÄSER POTENTIOMETERN OCH MAPPAR DET TILL LÄGSTA OCH HÖGSTA HASTIGHET
int hastighet = map(analogRead(POT), 0, 1023, MIN_SPEED, MAX_SPEED);
LÄSER DEN NUVARANDE SENSORN FÖR ATT BALANSERA BELASTNINGEN GÅENDE IN I BÅDE MOTORER
Double I_A = (((double)analogRead(CURRENT_A)) * 5/1023.0) - 2,5) / 0.1;
Double I_B = (((double)analogRead(CURRENT_B)) * 5/1023.0) - 2,5) / 0.1;
OM INGA KNAPPAR NERE, STOPPA MOTORERNA
om (DIR == B1111) {
stop_all = sant;
}
om (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);
}
om (LEFT_SPEED == 0 & & RIGHT_SPEED == 0 & & B1111) {
stop_all = false;
OLD_DIR = DIR;
}
OM MOTORERNA ÄR INAKTIVA, LYSSNA FÖR CONTROLLER INGÅNG
om (stop_all == false) {
om (DIR! = OLD_DIR & & digitalRead(FORWARD)! = B0) {
DIR = OLD_DIR;
}
Switch(Dir) {
fall B0111: //FORWARD
Nuvarande sensor ersättning
om (abs(I_A-I_B) > 0,1) {
om (I_A > I_B) {
COMP_A = (I_A-I_B) * HASTIGHET/TIME_TO_MAX_SPEED * FÖRSENING * 0,1;
COMP_B = (I_A-I_B) *-HASTIGHET/TIME_TO_MAX_SPEED * FÖRSENING * 0,1;
}
annat {
COMP_A = (I_A-I_B) *-HASTIGHET/TIME_TO_MAX_SPEED * FÖRSENING * 0,1;
COMP_B = (I_A-I_B) * HASTIGHET/TIME_TO_MAX_SPEED * FÖRSENING * 0,1;
}
}
annat {
COMP_A = 0;
COMP_B = 0;
}
beräkna acceleration
ACC = (SPEED-LEFT_SPEED) * 0,1;
ACC = (acc > = 6)? 6: acc;
ACC = (acc < = 1)? 1: acc;
Lägg till acceleration och ersättning till vänster och höger motors
LEFT_SPEED = LEFT_SPEED + acc + COMP_B;
RIGHT_SPEED = RIGHT_SPEED + acc + COMP_A;
LEFT_SPEED = (LEFT_SPEED > = hastighet)? HASTIGHET: LEFT_SPEED;
RIGHT_SPEED = (RIGHT_SPEED > = hastighet)? HASTIGHET: RIGHT_SPEED;
Ställa in motor riktningar, 1 = framåt, -1 = bakåt
LEFT_MOTOR_DIR = 1;
RIGHT_MOTOR_DIR = 1;
bryta;
fall B0101: //steer kvar när framåt
LEFT_SPEED = LEFT_SPEED * 0,9;
RIGHT_SPEED = RIGHT_SPEED;
bryta;
fall B0110: //steer höger när framåt
LEFT_SPEED = LEFT_SPEED;
RIGHT_SPEED = RIGHT_SPEED * 0,9;
bryta;
fall B1011: //REVERSE
Nuvarande sensor ersättning
om (abs(I_A-I_B) > 0,1) {
om (I_A > I_B) {
COMP_A = (I_A-I_B) * HASTIGHET/TIME_TO_MAX_SPEED * FÖRSENING * 0,1;
COMP_B = (I_A-I_B) *-HASTIGHET/TIME_TO_MAX_SPEED * FÖRSENING * 0,1;
}
annat {
COMP_A = (I_A-I_B) *-HASTIGHET/TIME_TO_MAX_SPEED * FÖRSENING * 0,1;
COMP_B = (I_A-I_B) * HASTIGHET/TIME_TO_MAX_SPEED * FÖRSENING * 0,1;
}
}
annat {
COMP_A = 0;
COMP_B = 0;
}
beräkna acceleration
ACC = (SPEED-LEFT_SPEED) * 0,1;
ACC = (acc > = 6)? 6: acc;
ACC = (acc < = 1)? 1: acc;
Lägg till acceleration och ersättning till vänster och höger motors
LEFT_SPEED = LEFT_SPEED + acc + COMP_B;
RIGHT_SPEED = RIGHT_SPEED + acc + COMP_A;
LEFT_SPEED = (LEFT_SPEED > = hastighet)? HASTIGHET: LEFT_SPEED;
RIGHT_SPEED = (RIGHT_SPEED > = hastighet)? HASTIGHET: RIGHT_SPEED;
Ställa in motor riktningar, 1 = framåt, -1 = bakåt
LEFT_MOTOR_DIR = -1;
RIGHT_MOTOR_DIR = -1;
bryta;
fall B1101: //LEFT
beräkna acceleration
ACC = (SPEED-LEFT_SPEED) * 0,1;
ACC = (acc > = 15)? 8: acc;
ACC = (acc < = 1)? 1: acc;
Lägg till acceleration och ersättning till vänster och höger motors
LEFT_SPEED = LEFT_SPEED + acc;
RIGHT_SPEED = RIGHT_SPEED + acc;
LEFT_SPEED = (LEFT_SPEED > = hastighet)? HASTIGHET: LEFT_SPEED;
RIGHT_SPEED = (RIGHT_SPEED > = hastighet)? HASTIGHET: RIGHT_SPEED;
Ställa in motor riktningar, 1 = framåt, -1 = bakåt
LEFT_MOTOR_DIR = -1;
RIGHT_MOTOR_DIR = 1;
bryta;
fall B1110: //RIGHT
beräkna acceleration
ACC = (SPEED-LEFT_SPEED) * 0,1;
ACC = (acc > = 15)? 8: acc;
ACC = (acc < = 1)? 1: acc;
Lägg till acceleration och ersättning till vänster och höger motors
LEFT_SPEED = LEFT_SPEED + acc;
RIGHT_SPEED = RIGHT_SPEED + acc;
LEFT_SPEED = (LEFT_SPEED > = hastighet)? HASTIGHET: LEFT_SPEED;
RIGHT_SPEED = (RIGHT_SPEED > = hastighet)? HASTIGHET: RIGHT_SPEED;
Ställa in motor riktningar, 1 = framåt, -1 = bakåt
LEFT_MOTOR_DIR = 1;
RIGHT_MOTOR_DIR = -1;
bryta;
}
}
Ställ in motor riktningar
om (RIGHT_MOTOR_DIR > 0) {
digitalWrite (DIR_A, låg);
}
annat {
digitalWrite (DIR_A, hög);
}
om (LEFT_MOTOR_DIR > 0) {
digitalWrite (DIR_B, låg);
}
annat {
digitalWrite (DIR_B, hög);
}
Ange varvtal
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);
}
Anger frekvensen för PWM
void setPwmFrequency (int pin, int divisor) {
byte läge;
om (pin == 5 || pin == 6 || pin == 9 || pin == 10) {
Switch(divisor) {
fall 1:
läge = 0x01;
bryta;
mål 8:
läge = 0x02;
bryta;
fall 64:
läge = 0x03;
bryta;
fall 256:
läge = 0x04;
bryta;
fall 1024:
läge = 0x05;
bryta;
standard:
hemkomst.
}
om (pin == 5 || pin == 6) {
TCCR0B = TCCR0B & 0b11111000 | läge;
}
annat {
TCCR1B = TCCR1B & 0b11111000 | läge;
}
}
annars om (pin == 3 || pin == 11) {
Switch(divisor) {
fall 1:
läge = 0x01;
bryta;
mål 8:
läge = 0x02;
bryta;
fall 32:
läge = 0x03;
bryta;
fall 64:
läge = 0x04;
bryta;
fall 128:
läge = 0x05;
bryta;
fall 256:
läge = 0x06;
bryta;
fall 1024:
läge = 0x7;
bryta;
standard:
hemkomst.
}
TCCR2B = TCCR2B & 0b11111000 | läge;
}
}

slutet av koden