Code 4
Code 4
/*
* Robot_SimpleMotorDrive
*
* Adafruit Feather M4 using Pololu TB6612FNG motor controller
*
* Motor Control Table
* XIN1 XIN2 Effect
* Low Low Brake
* Low High Forward
* High Low Reverse
*
* Free to use for all
* F Milburn, January 2020
*/
// Pins used to control motors
const uint16_t PWMA = 5; // Motor A PWM control Orange
const uint16_t AIN2 = 6; // Motor A input 2 Brown
const uint16_t AIN1 = 9; // Motor A input 1 Green
const uint16_t BIN1 = 10; // Motor B input 1 Yellow
const uint16_t BIN2 = 11; // Motor B input 2 Purple
const uint16_t PWMB = 12; // Motor B PWM control White
const uint16_t STBY = 13; // Standby Brown
// Constants
const uint16_t ANALOG_WRITE_BITS = 8;
const uint16_t MAX_PWM = pow(2, ANALOG_WRITE_BITS);
const uint16_t MIN_PWM = MAX_PWM / 3; // Make sure motor turns
void setup(){
initMotors();
}
void loop(){
uint16_t pwm = 0;
// test forward
for (pwm = MIN_PWM; pwm <= MAX_PWM; pwm += 20){
forwardA(pwm);
forwardB(pwm);
delay(2000);
}
// brake
brakeA();
brakeB();
delay(500);
// test reverse(){
for (pwm = MIN_PWM; pwm <= MAX_PWM; pwm += 20){
reverseA(pwm);
reverseB(pwm);
delay(2000);
}
// brake
brakeA();
brakeB();
delay(500);
}
void forwardA(uint16_t pwm){
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, HIGH);
if (pwm > MAX_PWM){
pwm = MAX_PWM;
}
if (pwm < MIN_PWM){
pwm = MIN_PWM;
}
analogWrite(PWMA, pwm);
}
void forwardB(uint16_t pwm){
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, HIGH);
if (pwm > MAX_PWM){
pwm = MAX_PWM;
}
if (pwm < MIN_PWM){
pwm = MIN_PWM;
}
analogWrite(PWMB, pwm);
}
void reverseA(uint16_t pwm){
digitalWrite(AIN1, HIGH);
digitalWrite(AIN2, LOW);
if (pwm > MAX_PWM){
pwm = MAX_PWM;
}
if (pwm < MIN_PWM){
pwm = MIN_PWM;
}
analogWrite(PWMA, pwm);
}
void reverseB(uint16_t pwm){
digitalWrite(BIN1, HIGH);
digitalWrite(BIN2, LOW);
if (pwm > MAX_PWM){
pwm = MAX_PWM;
}
if (pwm < MIN_PWM){
pwm = MIN_PWM;
}
analogWrite(PWMB, pwm);
}
void brakeA(){
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, LOW);
}
void brakeB(){
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, LOW);
}
void standbyMotors(bool standby){
if (standby == true){
digitalWrite(STBY, LOW);
}
else{
digitalWrite(STBY, HIGH);
}
}
void initMotors(){
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
pinMode(PWMA, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);
pinMode(PWMB, OUTPUT);
pinMode(STBY, OUTPUT);
analogWriteResolution(ANALOG_WRITE_BITS);
standbyMotors(false);
}