/* Initio 03 Uses PWM speed control for the initio 4WD robot platform Uses the speed sensors to count distance moved forwards, then moves the same distance in reverse Purpose: Demo use of speed sensors This example code is in the public domain. */ int L1 = 6, L2 = 7, L3 = 9, L4 =8; // Output Pins connected to Motor driver L298N H-Bridge module // L1 and L3 must be connected to PWM pins int DL0 = 3, DL1 = 5, DR0 = 2, DR1 = 4; // Pins for Right and Left encoders. DL0 and DR0 must be Interrupt pins int lStat0, lStat1, rStat0, rStat1; // variables to hold status of each speed sensor input int lpos = 0, rpos = 0; // variables keeping count of the pulses on each side String dir = "None"; void intLeft() // This interrupt routine runs if the left side encoder is triggered (DL0) { lStat0 = digitalRead(DL0); lStat1 = digitalRead(DL1); if(lStat1 > 0) // use DL1 to determine which direction we're moving. HIGH = Forwards lpos++; else lpos--; dir = "Left"; } void intRight() // This interrupt routine runs if the right side encoder is triggered (DR0) { rStat0 = digitalRead(DR0); rStat1 = digitalRead(DR1); if(rStat1 > 0) // use DR1 to determine which direction we're moving. HIGH = Backwards rpos--; else rpos++; dir = "Right"; } // the setup routine runs once when you press reset: void setup() { // initialize the digital pins we will use to drive motors pinMode(L1, OUTPUT); pinMode(L2, OUTPUT); pinMode(L3, OUTPUT); pinMode(L4, OUTPUT); // initialise input pins and interrupts for speed sensors pinMode(DL0, INPUT); pinMode(DL1, INPUT); pinMode(DR0, INPUT); pinMode(DR1, INPUT); attachInterrupt(1, intLeft, RISING); attachInterrupt(0, intRight, RISING); Serial.begin(115200); } // the loop routine runs over and over again forever: void loop() { lpos = rpos = 0; // reset position counters forward(2000, 255); // Move forward at top speed for 2 seconds halt(500); while (rpos > 0) // only check the right counter for this simple test { if (rpos > 50) reverse (20, 160); else reverse (20, 100); // slow down when near the end } halt(3000); } // robMove routine switches the correct inputs to the L298N for the direction selected. void robMove(int l1, int l2, int r1, int r2) { analogWrite(L1, l1); digitalWrite(L2, l2); analogWrite(L3, r1); digitalWrite(L4, r2); } void reverse(int wait, int vSpeed) { Serial.println("Moving backward"); robMove(255-vSpeed, HIGH, 255-vSpeed, HIGH); // when reversing, the speed needs to be opposite, so subtract from 255 delay(wait); } void forward(int wait, int vSpeed) { Serial.println("Moving forward"); robMove(vSpeed, LOW, vSpeed, LOW); delay(wait); } void rightTurnFd(int wait, int lSpeed, int rSpeed) { Serial.println("Turning right forward"); robMove(lSpeed, LOW, rSpeed, LOW); delay(wait); } void leftTurnFd(int wait, int lSpeed, int rSpeed) { Serial.println("Turning left forward"); robMove(lSpeed, LOW, rSpeed, LOW); delay(wait); } void rightTurnBd(int wait, int lSpeed, int rSpeed) { Serial.println("Turning right backward"); robMove(255-lSpeed, HIGH, 255-rSpeed, HIGH); delay(wait); } void leftTurnBd(int wait, int lSpeed, int rSpeed) { Serial.println("Turning left backward"); robMove(255-lSpeed, HIGH, 255-rSpeed, HIGH); delay(wait); } void halt(int wait) { Serial.println("Stopping"); robMove(0, LOW, 0, LOW); delay(wait); }