/*
Code by Brian Patton
4/27/2016
4Wire PWM DC motor control
*/
byte In1pin = 9; //Teensy pin
byte In2pin = 10; //Teensy pin
byte In3pin = 11; //Teensy pin
byte In4pin = 12; //Teensy pin
byte En1pin = 3; //Teensy PWM pin
byte En2pin = 4; //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(En1pin, OUTPUT); // Set pin to Output
pinMode(En2pin, 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);
analogWrite(En1pin, PWM_s); //Left Forward PWM 2 of 2 //Short head start if running on USB
analogWrite(En2pin, PWM_s); //Right Forward PWM 1 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);
}
}