Code Example


/*
   Code by Brian Patton
   4/27/2016
   4Wire PWM DC motor control
*/
byte In1pin = 3;  //Teensy PWM pin
byte In2pin = 4;  //Teensy PWM pin
byte In3pin = 5;  //Teensy PWM pin
byte In4pin = 6;  //Teensy PWM pin

byte Redpin = 20;  //Red LED Pin
byte Greenpin = 21;//Green LED Pin
byte Bluepin = 22; //Blue LED Pin

int PWMspeed;
//********************************************************
//    Setup
//********************************************************
void setup() {
  // put your setup code here, to run once:
  pinMode(In1pin, OUTPUT);   // Set pin to Output
  pinMode(In2pin, OUTPUT);   // Set pin to Output
  pinMode(In3pin, OUTPUT);   // Set pin to Output
  pinMode(In4pin, OUTPUT);   // Set pin to Output
  pinMode(Redpin, OUTPUT);   // Set pin to Output
  pinMode(Greenpin, OUTPUT); // Set pin to Output
  pinMode(Bluepin, OUTPUT);  // Set pin to Output
  pinMode(13, OUTPUT);       // Set pin to Output
}
//********************************************************
//    Go Forward for a time
//********************************************************
void Forward(int dlay, int PWM_s) {
  digitalWrite(In1pin, LOW);  //Left Forward 1 of 2
  digitalWrite(In2pin, HIGH); //Left Forward 2 of 2
  delay(10);
  digitalWrite(In3pin, HIGH); //Right Forward 1 of 2
  digitalWrite(In4pin, LOW);  //Right Forward 2 of 2
  delay(10);
  digitalWrite(In1pin, LOW);  //Left Forward 1 of 2
  analogWrite(In2pin, PWM_s); //Left Forward PWM 2 of 2               //Short head start if running on USB
  analogWrite(In3pin, PWM_s); //Right Forward PWM 1 of 2
  digitalWrite(In4pin, LOW);  //Right Forward 2 of 2

  analogWrite(Redpin, PWM_s - 60); //Dim Light

  delay(dlay);
}
//********************************************************
//    Turn Right for a time
//********************************************************
void TRight(int dlay) {
  digitalWrite(In1pin, LOW);  //Left Forward 1 of 2
  digitalWrite(In2pin, HIGH); //Left Forward 2 of 2
  delay(5);                   //Short head start if running on USB
  digitalWrite(In3pin, LOW);  //Right Reverse 1 of 2
  digitalWrite(In4pin, HIGH); //Right Reverse 2 of 2
  delay(dlay);
}
//********************************************************
//    Turn Left for a time
//********************************************************
void TLeft(int dlay) {
  digitalWrite(In1pin, HIGH); //Left Reverse 1 of 2
  digitalWrite(In2pin, LOW);  //Left Reverse 2 of 2
  delay(5);                   //Short head start if running on USB
  digitalWrite(In3pin, HIGH); //Right Forward 1 of 2
  digitalWrite(In4pin, LOW);  //Right Forward 2 of 2
  delay(dlay);
}
//********************************************************
//    Reverse for a time
//********************************************************
void Reverse(int dlay) {
  digitalWrite(In1pin, HIGH); //Left Reverse 1 of 2
  digitalWrite(In2pin, LOW);  //Left Reverse 2 of 2
  delay(5);                   //Short head start if running on USB
  digitalWrite(In3pin, LOW);  //Right Reverse 1 of 2
  digitalWrite(In4pin, HIGH); //Right Reverse 2 of 2
  delay(dlay);
}
//********************************************************
//    Stop for a time
//********************************************************
void StopBot(int dlay) {
  //All pins low to stop
  analogWrite(In1pin, 0);
  analogWrite(In2pin, 0);
  analogWrite(In3pin, 0);
  analogWrite(In4pin, 0);
  delay(dlay);
}
//********************************************************
//    Stop with infinite loop
//********************************************************
void FinalStop() {
  //All pins low to stop
  digitalWrite(In1pin, LOW);
  digitalWrite(In2pin, LOW);
  digitalWrite(In3pin, LOW);
  digitalWrite(In4pin, LOW);
  while (true) {
    digitalWrite(13, HIGH);
    delay(500);
    digitalWrite(13, LOW);
    delay(500);
  }
}
//********************************************************
//    Main Loop
//********************************************************
void loop() {
  for (int i = 60; i <= 254; i += 5) {
    Forward(200, i);
  }
  for (int i = 254; i >= 60; i-= 5) {
    Forward(200, i);
  }


}