//WNQ 12.22.2016 //L298N Motor Controller Code by Reichenstein7 (thejamerson.com) //Sonar code from https://sites.google.com/site/myscratchbooks/home/projects/project-09-ultrasonic-sensor // Motor 1 int dir1PinA = 6; //L298N pin "in1" - arduino digital pin "6" int dir2PinA = 5; //L298N pin "in2" - arduino digital pin "6" int speedPinA = 7; //L298N pin "ENA" - arduino digital pin "7", Needs to be a PWM pin to be able to control motor speed // Motor 2 int dir1PinB = 10; //L298N pin "in3" - arduino digital pin "10" int dir2PinB = 9; //L298N pin "in4" - arduino digital pin "9" int speedPinB = 11; //L298N pin "ENB" - arduino digital pin "11", Needs to be a PWM pin to be able to control motor speed //L298N pin "+5V" - arduino pin "vin" for give powaer int _speedOriginal = 60; //min:48 int _randomRate = _speedOriginal * 0.2; int _speed = _speedOriginal; //min:48 //sonar init unsigned int EchoPin = 2; // The Arduino's the Pin2 connection to US-100 Echo / RX unsigned int TrigPin = 3; // The Arduino's Pin3 connected to US-100 Trig / TX unsigned long Time_Echo_us = 0; unsigned long _Len_mm = 500; void setup() { // Setup runs once per reset // initialize serial communication @ 9600 baud: Serial.begin(9600); //Define L298N Dual H-Bridge Motor Controller Pins pinMode(dir1PinA, OUTPUT); pinMode(dir2PinA, OUTPUT); pinMode(speedPinA, OUTPUT); pinMode(dir1PinB, OUTPUT); pinMode(dir2PinB, OUTPUT); pinMode(speedPinB, OUTPUT); //init sonar pinMode(EchoPin, INPUT); // The set EchoPin input mode. pinMode(TrigPin, OUTPUT); // The set TrigPin output mode. delay(1000); //wait for plug power } void loop() { sonarCheck(); // Scale speed by distance if (_Len_mm < 600) { float rate = (float)((int)_Len_mm - 300) / 3000.0; Serial.print(", rate : "); Serial.print(rate); _speed = _speedOriginal * rate; Serial.print(", _speed : "); Serial.println(_speed); if (_speed < 40 ) { _speed = 40; } } else { _speed = _speedOriginal; } if (_Len_mm < 300) { carControl('5', _speedOriginal); if (random(0, 2)) { carControl('3', _speedOriginal); } else { carControl('4', _speedOriginal); } } else { carControl('1', _speed); } } //1-forward, 2-backward, 3-left, 4-right void carControl(byte act, int sp1) { Serial.print("car act : "); Serial.println(act); switch (act) { case '1': motorCol('1', sp1); motorCol('4', sp1); break; case '2': motorCol('3', sp1); motorCol('6', sp1); break; case '3': motorCol('2', 0); motorCol('6', sp1); delay(500); break; case '4': motorCol('3', sp1); motorCol('5', 0); delay(500); case '5': motorCol('2', 0); motorCol('5', 0); delay(100); break; case '6': motorCol('1', random(sp1 - _randomRate, sp1 + _randomRate)); motorCol('4', random(sp1 - _randomRate, sp1 + _randomRate)); break; default: Serial.println("carCol do nothing"); } } void motorCol(byte sta, int sp) { switch (sta) { //______________Motor 1______________ case '1': // Motor 1 Forward analogWrite(speedPinA, sp);//Sets speed variable via PWM digitalWrite(dir1PinA, LOW); digitalWrite(dir2PinA, HIGH); //Serial.println("Motor 1 Forward"); // Prints out “Motor 1 Forward” on the serial monitor //Serial.println(" "); // Creates a blank line printed on the serial monitor break; case '2': // Motor 1 Stop (Freespin) analogWrite(speedPinA, 0); digitalWrite(dir1PinA, LOW); digitalWrite(dir2PinA, HIGH); //Serial.println("Motor 1 Stop"); //Serial.println(" "); break; case '3': // Motor 1 Reverse analogWrite(speedPinA, sp); digitalWrite(dir1PinA, HIGH); digitalWrite(dir2PinA, LOW); //Serial.println("Motor 1 Reverse"); //Serial.println(" "); break; //______________Motor 2______________ case '4': // Motor 2 Forward analogWrite(speedPinB, sp); digitalWrite(dir1PinB, LOW); digitalWrite(dir2PinB, HIGH); //Serial.println("Motor 2 Forward"); //Serial.println(" "); break; case '5': // Motor 1 Stop (Freespin) analogWrite(speedPinB, 0); digitalWrite(dir1PinB, LOW); digitalWrite(dir2PinB, HIGH); //Serial.println("Motor 2 Stop"); //Serial.println(" "); break; case '6': // Motor 2 Reverse analogWrite(speedPinB, sp); digitalWrite(dir1PinB, HIGH); digitalWrite(dir2PinB, LOW); //Serial.println("Motor 2 Reverse"); //Serial.println(" "); break; default: // turn all the connections off if an unmapped key is pressed: for (int thisPin = 2; thisPin < 11; thisPin++) { digitalWrite(thisPin, LOW); } } } void sonarCheck() { // By the Trig / Pin sending pulses trigger US-100 ranging digitalWrite(TrigPin, HIGH); // Send pulses begin by Trig / Pin delayMicroseconds(50); // Set the pulse width of 50us (> 10us) digitalWrite(TrigPin, LOW); // The end of the pulse Time_Echo_us = pulseIn(EchoPin, HIGH); // A pulse width calculating US-100 returned if ((Time_Echo_us < 60000) && (Time_Echo_us > 1)) { // Pulse effective range (1, 60000). // _Len_mm = (Time_Echo_us * 0.34mm/us) / 2 (mm) _Len_mm = (Time_Echo_us * 34 / 100) / 2; // Calculating the distance by a pulse width. Serial.print("Present Distance is: "); // Output to the serial port monitor Serial.print(_Len_mm, DEC); // Output to the serial port monitor Serial.println("mm"); // Output to the serial port monitor } delay(200); // Per second (1000ms) measured }