Stap 7: Elektronica: code uploaden
De volgende code uploaden naar de Arduino Uno-bestuur. Vindt u instructies over het instellen van uw arduino hier: https://www.arduino.cc/en/Guide/HomePage
begin van de code
Const int DIR_A = 2, //Motor Shield A DIR pin
DIR_B = 4, //Motor Shield B DIR pin
PWM_A = 3, //Motor Shield A PWM pin
PWM_B = 5, //Motor Shield A PWM pin
POT = A3, //Potentiometer pin
VOORWAARTS = 8, //Forward pin
KEREN = 9, //Reverse pin
Links = 11, //Left pin
RECHTS = 10, //Right pin
CURRENT_A = A1, //Current Sensor A pin
CURRENT_B = A0; Huidige pin van de Sensor B
int DIR, OLD_DIR; Controller Input en Input geschiedenis
Boole stop_all = false; Verlaag de snelheid op 0
Variabelen declareren
int MAX_SPEED = 230, MIN_SPEED = 60;
zweven acc = 0;
zweven LEFT_SPEED = 0;
zweven RIGHT_SPEED = 0;
zweven COMP_A = 0;
zweven COMP_B = 0;
int LEFT_MOTOR_DIR = 0;
int RIGHT_MOTOR_DIR = 0;
dubbele TIME_TO_MAX_SPEED = 5000;
int vertraging = 100;
VOID Setup {}
Definiëren van Input en Output pinnen
pinMode (DIR_A, uitvoer);
pinMode (DIR_B, uitvoer);
pinMode (PWM_A, uitvoer);
pinMode (PWM_B, uitvoer);
pinMode (recht, INPUT_PULLUP);
pinMode (LEFT, INPUT_PULLUP);
pinMode (FORWARD, INPUT_PULLUP);
pinMode (REVERSE, INPUT_PULLUP);
pinMode (POT, INPUT);
pinMode (CURRENT_A, INPUT);
pinMode (CURRENT_B, INPUT);
Initialiseert de seriële poort voor monitoring indien nodig
Serial.begin(9600);
DIR = (digitalRead(FORWARD) << 3)
+ (digitalRead(REVERSE) << 2)
+ (digitalRead(LEFT) << 1)
+ (digitalRead(RIGHT));
OLD_DIR = DIR;
Daling PWM frequentie om motor janken
setPwmFrequency(3,1);
setPwmFrequency(5,1);
}
void loop {}
Leest de controller input
VOORWAARTS: B0111
OMKEREN: B1011
LINKS: B1101
RECHTS: B1110
DIR = (digitalRead(FORWARD) << 3)
+ (digitalRead(REVERSE) << 2)
+ (digitalRead(LEFT) << 1)
+ (digitalRead(RIGHT));
LEEST DE POTENTIOMETER EN HET WORDT TOEGEWEZEN AAN DE MINIMUM- EN MAXIMUMSNELHEID
int SPEED = map(analogRead(POT), 0, 1023, MIN_SPEED, MAX_SPEED);
LEEST DE HUIDIGE SENSOR OM DE SERVERBELASTING GAAN BEIDE MOTOREN
dubbele I_A = (((double)analogRead(CURRENT_A)) * 5/1023.0) - 2,5) / 0,1;
dubbele I_B = (((double)analogRead(CURRENT_B)) * 5/1023.0) - 2,5) / 0,1;
ALS GEEN TOETSEN INGEDRUKT, STOPT DE MOTOREN
Als (DIR == B1111) {}
stop_all = true;
}
Als (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);
}
Als (LEFT_SPEED == 0 & & RIGHT_SPEED == 0 & & B1111) {}
stop_all = false;
OLD_DIR = DIR;
}
ALS MOTOREN NIET ACTIEF ZIJN, LUISTEREN CONTROLLER INVOERFILTER
Als (stop_all == false) {}
Als (DIR! = OLD_DIR & & digitalRead(FORWARD)! = B0) {}
DIR = OLD_DIR;
}
switch(dir) {}
kast B0111: //FORWARD
huidige sensor compensatie
Als (abs(I_A-I_B) > 0.1) {}
Als (I_A > I_B) {}
COMP_A = (I_A-I_B) * SNELHEID/TIME_TO_MAX_SPEED * VERTRAGING * 0.1;
COMP_B = (I_A-I_B) *-SNELHEID/TIME_TO_MAX_SPEED * VERTRAGING * 0.1;
}
else {}
COMP_A = (I_A-I_B) *-SNELHEID/TIME_TO_MAX_SPEED * VERTRAGING * 0.1;
COMP_B = (I_A-I_B) * SNELHEID/TIME_TO_MAX_SPEED * VERTRAGING * 0.1;
}
}
else {}
COMP_A = 0;
COMP_B = 0;
}
versnelling berekenen
ACC = (snelheid-LEFT_SPEED) * 0.1;
ACC = (acc > = 6)? 6: acc;
ACC = (acc < = 1)? 1: acc;
versnelling en compensatie toevoegen aan de linker- en motoren
LEFT_SPEED = LEFT_SPEED + acc + COMP_B;
RIGHT_SPEED = RIGHT_SPEED + acc + COMP_A;
LEFT_SPEED = (LEFT_SPEED > = snelheid)? SNELHEID: LEFT_SPEED;
RIGHT_SPEED = (RIGHT_SPEED > = snelheid)? SNELHEID: RIGHT_SPEED;
Stel motor richtingen, 1 = vooruit -1 = omgekeerde
LEFT_MOTOR_DIR = 1;
RIGHT_MOTOR_DIR = 1;
breken;
kast B0101: //steer verliet toen vooruit
LEFT_SPEED = LEFT_SPEED * 0.9;
RIGHT_SPEED = RIGHT_SPEED;
breken;
kast B0110: //steer gelijk toen vooruit
LEFT_SPEED = LEFT_SPEED;
RIGHT_SPEED = RIGHT_SPEED * 0.9;
breken;
kast B1011: //REVERSE
huidige sensor compensatie
Als (abs(I_A-I_B) > 0.1) {}
Als (I_A > I_B) {}
COMP_A = (I_A-I_B) * SNELHEID/TIME_TO_MAX_SPEED * VERTRAGING * 0.1;
COMP_B = (I_A-I_B) *-SNELHEID/TIME_TO_MAX_SPEED * VERTRAGING * 0.1;
}
else {}
COMP_A = (I_A-I_B) *-SNELHEID/TIME_TO_MAX_SPEED * VERTRAGING * 0.1;
COMP_B = (I_A-I_B) * SNELHEID/TIME_TO_MAX_SPEED * VERTRAGING * 0.1;
}
}
else {}
COMP_A = 0;
COMP_B = 0;
}
versnelling berekenen
ACC = (snelheid-LEFT_SPEED) * 0.1;
ACC = (acc > = 6)? 6: acc;
ACC = (acc < = 1)? 1: acc;
versnelling en compensatie toevoegen aan de linker- en motoren
LEFT_SPEED = LEFT_SPEED + acc + COMP_B;
RIGHT_SPEED = RIGHT_SPEED + acc + COMP_A;
LEFT_SPEED = (LEFT_SPEED > = snelheid)? SNELHEID: LEFT_SPEED;
RIGHT_SPEED = (RIGHT_SPEED > = snelheid)? SNELHEID: RIGHT_SPEED;
Stel motor richtingen, 1 = vooruit -1 = omgekeerde
LEFT_MOTOR_DIR = -1;
RIGHT_MOTOR_DIR = -1;
breken;
kast B1101: //LEFT
versnelling berekenen
ACC = (snelheid-LEFT_SPEED) * 0.1;
ACC = (acc > = 15)? 8: acc;
ACC = (acc < = 1)? 1: acc;
versnelling en compensatie toevoegen aan de linker- en motoren
LEFT_SPEED = LEFT_SPEED + acc;
RIGHT_SPEED = RIGHT_SPEED + acc;
LEFT_SPEED = (LEFT_SPEED > = snelheid)? SNELHEID: LEFT_SPEED;
RIGHT_SPEED = (RIGHT_SPEED > = snelheid)? SNELHEID: RIGHT_SPEED;
Stel motor richtingen, 1 = vooruit -1 = omgekeerde
LEFT_MOTOR_DIR = -1;
RIGHT_MOTOR_DIR = 1;
breken;
kast B1110: //RIGHT
versnelling berekenen
ACC = (snelheid-LEFT_SPEED) * 0.1;
ACC = (acc > = 15)? 8: acc;
ACC = (acc < = 1)? 1: acc;
versnelling en compensatie toevoegen aan de linker- en motoren
LEFT_SPEED = LEFT_SPEED + acc;
RIGHT_SPEED = RIGHT_SPEED + acc;
LEFT_SPEED = (LEFT_SPEED > = snelheid)? SNELHEID: LEFT_SPEED;
RIGHT_SPEED = (RIGHT_SPEED > = snelheid)? SNELHEID: RIGHT_SPEED;
Stel motor richtingen, 1 = vooruit -1 = omgekeerde
LEFT_MOTOR_DIR = 1;
RIGHT_MOTOR_DIR = -1;
breken;
}
}
set motor richtingen
Als (RIGHT_MOTOR_DIR > 0) {}
digitalWrite (DIR_A, laag);
}
else {}
digitalWrite (DIR_A, hoge);
}
Als (LEFT_MOTOR_DIR > 0) {}
digitalWrite (DIR_B, laag);
}
else {}
digitalWrite (DIR_B, hoge);
}
instellen van de motorsnelheid
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);
}
Hiermee stelt u de frequentie van de PWM
VOID setPwmFrequency (int pin, int deler) {}
byte modus;
Als (pin == 5 || pin 6 == || pin 9 == || pin 10 ==) {}
switch(divisor) {}
Case 1:
modus = 0x01;
breken;
zaak 8:
modus = 0x02;
breken;
Case 64:
modus = 0x03;
breken;
Case 256:
modus = 0x04;
breken;
Case 1024:
modus = 0x05;
breken;
standaard:
terugkeer;
}
Als (pin == 5 || pin 6 ==) {}
TCCR0B = TCCR0B & 0b11111000 | modus;
}
else {}
TCCR1B = TCCR1B & 0b11111000 | modus;
}
}
anders als (pin 3 == || pin == 11) {}
switch(divisor) {}
Case 1:
modus = 0x01;
breken;
zaak 8:
modus = 0x02;
breken;
Case 32:
modus = 0x03;
breken;
Case 64:
modus = 0x04;
breken;
Case 128:
modus = 0x05;
breken;
Case 256:
modus = 0x06;
breken;
Case 1024:
modus = 0x7;
breken;
standaard:
terugkeer;
}
TCCR2B = TCCR2B & 0b11111000 | modus;
}
}
einde van code