├── AFMotor.cpp
├── AFMotor.h
├── Obstacle Avoiding Robot using Arduino and L293d.jpg
├── Obstacle_Avoiding_Robot.ino
└── README.md


/AFMotor.cpp:
--------------------------------------------------------------------------------
  1 | #if (ARDUINO >= 100)
  2 |   #include "Arduino.h"
  3 | #else
  4 |   #if defined(__AVR__)
  5 |     #include <avr/io.h>
  6 |   #endif
  7 |   #include "WProgram.h"
  8 | #endif
  9 | 
 10 | #include "AFMotor.h"
 11 | 
 12 | 
 13 | 
 14 | static uint8_t latch_state;
 15 | 
 16 | #if (MICROSTEPS == 8)
 17 | uint8_t microstepcurve[] = {0, 50, 98, 142, 180, 212, 236, 250, 255};
 18 | #elif (MICROSTEPS == 16)
 19 | uint8_t microstepcurve[] = {0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255};
 20 | #endif
 21 | 
 22 | AFMotorController::AFMotorController(void) {
 23 |     TimerInitalized = false;
 24 | }
 25 | 
 26 | void AFMotorController::enable(void) {
 27 |   // setup the latch
 28 |   /*
 29 |   LATCH_DDR |= _BV(LATCH);
 30 |   ENABLE_DDR |= _BV(ENABLE);
 31 |   CLK_DDR |= _BV(CLK);
 32 |   SER_DDR |= _BV(SER);
 33 |   */
 34 |   pinMode(MOTORLATCH, OUTPUT);
 35 |   pinMode(MOTORENABLE, OUTPUT);
 36 |   pinMode(MOTORDATA, OUTPUT);
 37 |   pinMode(MOTORCLK, OUTPUT);
 38 | 
 39 |   latch_state = 0;
 40 | 
 41 |   latch_tx();  // "reset"
 42 | 
 43 |   //ENABLE_PORT &= ~_BV(ENABLE); // enable the chip outputs!
 44 |   digitalWrite(MOTORENABLE, LOW);
 45 | }
 46 | 
 47 | 
 48 | void AFMotorController::latch_tx(void) {
 49 |   uint8_t i;
 50 | 
 51 |   //LATCH_PORT &= ~_BV(LATCH);
 52 |   digitalWrite(MOTORLATCH, LOW);
 53 | 
 54 |   //SER_PORT &= ~_BV(SER);
 55 |   digitalWrite(MOTORDATA, LOW);
 56 | 
 57 |   for (i=0; i<8; i++) {
 58 |     //CLK_PORT &= ~_BV(CLK);
 59 |     digitalWrite(MOTORCLK, LOW);
 60 | 
 61 |     if (latch_state & _BV(7-i)) {
 62 |       //SER_PORT |= _BV(SER);
 63 |       digitalWrite(MOTORDATA, HIGH);
 64 |     } else {
 65 |       //SER_PORT &= ~_BV(SER);
 66 |       digitalWrite(MOTORDATA, LOW);
 67 |     }
 68 |     //CLK_PORT |= _BV(CLK);
 69 |     digitalWrite(MOTORCLK, HIGH);
 70 |   }
 71 |   //LATCH_PORT |= _BV(LATCH);
 72 |   digitalWrite(MOTORLATCH, HIGH);
 73 | }
 74 | 
 75 | static AFMotorController MC;
 76 | 
 77 | /******************************************
 78 |                MOTORS
 79 | ******************************************/
 80 | inline void initPWM1(uint8_t freq) {
 81 | #if defined(__AVR_ATmega8__) || \
 82 |     defined(__AVR_ATmega48__) || \
 83 |     defined(__AVR_ATmega88__) || \
 84 |     defined(__AVR_ATmega168__) || \
 85 |     defined(__AVR_ATmega328P__)
 86 |     // use PWM from timer2A on PB3 (Arduino pin #11)
 87 |     TCCR2A |= _BV(COM2A1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2a
 88 |     TCCR2B = freq & 0x7;
 89 |     OCR2A = 0;
 90 | #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
 91 |     // on arduino mega, pin 11 is now PB5 (OC1A)
 92 |     TCCR1A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc1a
 93 |     TCCR1B = (freq & 0x7) | _BV(WGM12);
 94 |     OCR1A = 0;
 95 | #elif defined(__PIC32MX__)
 96 |     #if defined(PIC32_USE_PIN9_FOR_M1_PWM)
 97 |         // Make sure that pin 11 is an input, since we have tied together 9 and 11
 98 |         pinMode(9, OUTPUT);
 99 |         pinMode(11, INPUT);
100 |         if (!MC.TimerInitalized)
101 |         {   // Set up Timer2 for 80MHz counting fro 0 to 256
102 |             T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=<freq>, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0
103 |             TMR2 = 0x0000;
104 |             PR2 = 0x0100;
105 |             MC.TimerInitalized = true;
106 |         }
107 |          // Setup OC4 (pin 9) in PWM mode, with Timer2 as timebase
108 |         OC4CON = 0x8006;    // OC32 = 0, OCTSEL=0, OCM=6
109 |         OC4RS = 0x0000;
110 |         OC4R = 0x0000;
111 |     #elif defined(PIC32_USE_PIN10_FOR_M1_PWM)
112 |         // Make sure that pin 11 is an input, since we have tied together 9 and 11
113 |         pinMode(10, OUTPUT);
114 |         pinMode(11, INPUT);
115 |         if (!MC.TimerInitalized)
116 |         {   // Set up Timer2 for 80MHz counting fro 0 to 256
117 |             T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=<freq>, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0
118 |             TMR2 = 0x0000;
119 |             PR2 = 0x0100;
120 |             MC.TimerInitalized = true;
121 |         }
122 |          // Setup OC5 (pin 10) in PWM mode, with Timer2 as timebase
123 |         OC5CON = 0x8006;    // OC32 = 0, OCTSEL=0, OCM=6
124 |         OC5RS = 0x0000;
125 |         OC5R = 0x0000;
126 |     #else
127 |         // If we are not using PWM for pin 11, then just do digital
128 |         digitalWrite(11, LOW);
129 |     #endif
130 | #else
131 |    #error "This chip is not supported!"
132 | #endif
133 |     #if !defined(PIC32_USE_PIN9_FOR_M1_PWM) && !defined(PIC32_USE_PIN10_FOR_M1_PWM)
134 |         pinMode(11, OUTPUT);
135 |     #endif
136 | }
137 | 
138 | inline void setPWM1(uint8_t s) {
139 | #if defined(__AVR_ATmega8__) || \
140 |     defined(__AVR_ATmega48__) || \
141 |     defined(__AVR_ATmega88__) || \
142 |     defined(__AVR_ATmega168__) || \
143 |     defined(__AVR_ATmega328P__)
144 |     // use PWM from timer2A on PB3 (Arduino pin #11)
145 |     OCR2A = s;
146 | #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
147 |     // on arduino mega, pin 11 is now PB5 (OC1A)
148 |     OCR1A = s;
149 | #elif defined(__PIC32MX__)
150 |     #if defined(PIC32_USE_PIN9_FOR_M1_PWM)
151 |         // Set the OC4 (pin 9) PMW duty cycle from 0 to 255
152 |         OC4RS = s;
153 |     #elif defined(PIC32_USE_PIN10_FOR_M1_PWM)
154 |         // Set the OC5 (pin 10) PMW duty cycle from 0 to 255
155 |         OC5RS = s;
156 |     #else
157 |         // If we are not doing PWM output for M1, then just use on/off
158 |         if (s > 127)
159 |         {
160 |             digitalWrite(11, HIGH);
161 |         }
162 |         else
163 |         {
164 |             digitalWrite(11, LOW);
165 |         }
166 |     #endif
167 | #else
168 |    #error "This chip is not supported!"
169 | #endif
170 | }
171 | 
172 | inline void initPWM2(uint8_t freq) {
173 | #if defined(__AVR_ATmega8__) || \
174 |     defined(__AVR_ATmega48__) || \
175 |     defined(__AVR_ATmega88__) || \
176 |     defined(__AVR_ATmega168__) || \
177 |     defined(__AVR_ATmega328P__)
178 |     // use PWM from timer2B (pin 3)
179 |     TCCR2A |= _BV(COM2B1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2b
180 |     TCCR2B = freq & 0x7;
181 |     OCR2B = 0;
182 | #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
183 |     // on arduino mega, pin 3 is now PE5 (OC3C)
184 |     TCCR3A |= _BV(COM1C1) | _BV(WGM10); // fast PWM, turn on oc3c
185 |     TCCR3B = (freq & 0x7) | _BV(WGM12);
186 |     OCR3C = 0;
187 | #elif defined(__PIC32MX__)
188 |     if (!MC.TimerInitalized)
189 |     {   // Set up Timer2 for 80MHz counting fro 0 to 256
190 |         T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=<freq>, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0
191 |         TMR2 = 0x0000;
192 |         PR2 = 0x0100;
193 |         MC.TimerInitalized = true;
194 |     }
195 |     // Setup OC1 (pin3) in PWM mode, with Timer2 as timebase
196 |     OC1CON = 0x8006;    // OC32 = 0, OCTSEL=0, OCM=6
197 |     OC1RS = 0x0000;
198 |     OC1R = 0x0000;
199 | #else
200 |    #error "This chip is not supported!"
201 | #endif
202 | 
203 |     pinMode(3, OUTPUT);
204 | }
205 | 
206 | inline void setPWM2(uint8_t s) {
207 | #if defined(__AVR_ATmega8__) || \
208 |     defined(__AVR_ATmega48__) || \
209 |     defined(__AVR_ATmega88__) || \
210 |     defined(__AVR_ATmega168__) || \
211 |     defined(__AVR_ATmega328P__)
212 |     // use PWM from timer2A on PB3 (Arduino pin #11)
213 |     OCR2B = s;
214 | #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
215 |     // on arduino mega, pin 11 is now PB5 (OC1A)
216 |     OCR3C = s;
217 | #elif defined(__PIC32MX__)
218 |     // Set the OC1 (pin3) PMW duty cycle from 0 to 255
219 |     OC1RS = s;
220 | #else
221 |    #error "This chip is not supported!"
222 | #endif
223 | }
224 | 
225 | inline void initPWM3(uint8_t freq) {
226 | #if defined(__AVR_ATmega8__) || \
227 |     defined(__AVR_ATmega48__) || \
228 |     defined(__AVR_ATmega88__) || \
229 |     defined(__AVR_ATmega168__) || \
230 |     defined(__AVR_ATmega328P__)
231 |     // use PWM from timer0A / PD6 (pin 6)
232 |     TCCR0A |= _BV(COM0A1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on OC0A
233 |     //TCCR0B = freq & 0x7;
234 |     OCR0A = 0;
235 | #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
236 |     // on arduino mega, pin 6 is now PH3 (OC4A)
237 |     TCCR4A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc4a
238 |     TCCR4B = (freq & 0x7) | _BV(WGM12);
239 |     //TCCR4B = 1 | _BV(WGM12);
240 |     OCR4A = 0;
241 | #elif defined(__PIC32MX__)
242 |     if (!MC.TimerInitalized)
243 |     {   // Set up Timer2 for 80MHz counting fro 0 to 256
244 |         T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=<freq>, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0
245 |         TMR2 = 0x0000;
246 |         PR2 = 0x0100;
247 |         MC.TimerInitalized = true;
248 |     }
249 |     // Setup OC3 (pin 6) in PWM mode, with Timer2 as timebase
250 |     OC3CON = 0x8006;    // OC32 = 0, OCTSEL=0, OCM=6
251 |     OC3RS = 0x0000;
252 |     OC3R = 0x0000;
253 | #else
254 |    #error "This chip is not supported!"
255 | #endif
256 |     pinMode(6, OUTPUT);
257 | }
258 | 
259 | inline void setPWM3(uint8_t s) {
260 | #if defined(__AVR_ATmega8__) || \
261 |     defined(__AVR_ATmega48__) || \
262 |     defined(__AVR_ATmega88__) || \
263 |     defined(__AVR_ATmega168__) || \
264 |     defined(__AVR_ATmega328P__)
265 |     // use PWM from timer0A on PB3 (Arduino pin #6)
266 |     OCR0A = s;
267 | #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
268 |     // on arduino mega, pin 6 is now PH3 (OC4A)
269 |     OCR4A = s;
270 | #elif defined(__PIC32MX__)
271 |     // Set the OC3 (pin 6) PMW duty cycle from 0 to 255
272 |     OC3RS = s;
273 | #else
274 |    #error "This chip is not supported!"
275 | #endif
276 | }
277 | 
278 | 
279 | 
280 | inline void initPWM4(uint8_t freq) {
281 | #if defined(__AVR_ATmega8__) || \
282 |     defined(__AVR_ATmega48__) || \
283 |     defined(__AVR_ATmega88__) || \
284 |     defined(__AVR_ATmega168__) || \
285 |     defined(__AVR_ATmega328P__)
286 |     // use PWM from timer0B / PD5 (pin 5)
287 |     TCCR0A |= _BV(COM0B1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on oc0a
288 |     //TCCR0B = freq & 0x7;
289 |     OCR0B = 0;
290 | #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
291 |     // on arduino mega, pin 5 is now PE3 (OC3A)
292 |     TCCR3A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc3a
293 |     TCCR3B = (freq & 0x7) | _BV(WGM12);
294 |     //TCCR4B = 1 | _BV(WGM12);
295 |     OCR3A = 0;
296 | #elif defined(__PIC32MX__)
297 |     if (!MC.TimerInitalized)
298 |     {   // Set up Timer2 for 80MHz counting fro 0 to 256
299 |         T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=<freq>, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0
300 |         TMR2 = 0x0000;
301 |         PR2 = 0x0100;
302 |         MC.TimerInitalized = true;
303 |     }
304 |     // Setup OC2 (pin 5) in PWM mode, with Timer2 as timebase
305 |     OC2CON = 0x8006;    // OC32 = 0, OCTSEL=0, OCM=6
306 |     OC2RS = 0x0000;
307 |     OC2R = 0x0000;
308 | #else
309 |    #error "This chip is not supported!"
310 | #endif
311 |     pinMode(5, OUTPUT);
312 | }
313 | 
314 | inline void setPWM4(uint8_t s) {
315 | #if defined(__AVR_ATmega8__) || \
316 |     defined(__AVR_ATmega48__) || \
317 |     defined(__AVR_ATmega88__) || \
318 |     defined(__AVR_ATmega168__) || \
319 |     defined(__AVR_ATmega328P__)
320 |     // use PWM from timer0A on PB3 (Arduino pin #6)
321 |     OCR0B = s;
322 | #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
323 |     // on arduino mega, pin 6 is now PH3 (OC4A)
324 |     OCR3A = s;
325 | #elif defined(__PIC32MX__)
326 |     // Set the OC2 (pin 5) PMW duty cycle from 0 to 255
327 |     OC2RS = s;
328 | #else
329 |    #error "This chip is not supported!"
330 | #endif
331 | }
332 | 
333 | AF_DCMotor::AF_DCMotor(uint8_t num, uint8_t freq) {
334 |   motornum = num;
335 |   pwmfreq = freq;
336 | 
337 |   MC.enable();
338 | 
339 |   switch (num) {
340 |   case 1:
341 |     latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B); // set both motor pins to 0
342 |     MC.latch_tx();
343 |     initPWM1(freq);
344 |     break;
345 |   case 2:
346 |     latch_state &= ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // set both motor pins to 0
347 |     MC.latch_tx();
348 |     initPWM2(freq);
349 |     break;
350 |   case 3:
351 |     latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B); // set both motor pins to 0
352 |     MC.latch_tx();
353 |     initPWM3(freq);
354 |     break;
355 |   case 4:
356 |     latch_state &= ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // set both motor pins to 0
357 |     MC.latch_tx();
358 |     initPWM4(freq);
359 |     break;
360 |   }
361 | }
362 | 
363 | void AF_DCMotor::run(uint8_t cmd) {
364 |   uint8_t a, b;
365 |   switch (motornum) {
366 |   case 1:
367 |     a = MOTOR1_A; b = MOTOR1_B; break;
368 |   case 2:
369 |     a = MOTOR2_A; b = MOTOR2_B; break;
370 |   case 3:
371 |     a = MOTOR3_A; b = MOTOR3_B; break;
372 |   case 4:
373 |     a = MOTOR4_A; b = MOTOR4_B; break;
374 |   default:
375 |     return;
376 |   }
377 |   
378 |   switch (cmd) {
379 |   case BACKWARD:
380 |     latch_state |= _BV(a);
381 |     latch_state &= ~_BV(b); 
382 |     MC.latch_tx();
383 |     break;
384 |   case FORWARD:
385 |     latch_state &= ~_BV(a);
386 |     latch_state |= _BV(b); 
387 |     MC.latch_tx();
388 |     break;
389 |   case RELEASE:
390 |     latch_state &= ~_BV(a);     // A and B both low
391 |     latch_state &= ~_BV(b); 
392 |     MC.latch_tx();
393 |     break;
394 |   }
395 | }
396 | 
397 | void AF_DCMotor::setSpeed(uint8_t speed) {
398 |   switch (motornum) {
399 |   case 1:
400 |     setPWM1(speed); break;
401 |   case 2:
402 |     setPWM2(speed); break;
403 |   case 3:
404 |     setPWM3(speed); break;
405 |   case 4:
406 |     setPWM4(speed); break;
407 |   }
408 | }
409 | 
410 | /******************************************
411 |                STEPPERS
412 | ******************************************/
413 | 
414 | AF_Stepper::AF_Stepper(uint16_t steps, uint8_t num) {
415 |   MC.enable();
416 | 
417 |   revsteps = steps;
418 |   steppernum = num;
419 |   currentstep = 0;
420 | 
421 |   if (steppernum == 1) {
422 |     latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) &
423 |       ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0
424 |     MC.latch_tx();
425 |     
426 |     // enable both H bridges
427 |     pinMode(11, OUTPUT);
428 |     pinMode(3, OUTPUT);
429 |     digitalWrite(11, HIGH);
430 |     digitalWrite(3, HIGH);
431 | 
432 |     // use PWM for microstepping support
433 |     initPWM1(STEPPER1_PWM_RATE);
434 |     initPWM2(STEPPER1_PWM_RATE);
435 |     setPWM1(255);
436 |     setPWM2(255);
437 | 
438 |   } else if (steppernum == 2) {
439 |     latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) &
440 |       ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0
441 |     MC.latch_tx();
442 | 
443 |     // enable both H bridges
444 |     pinMode(5, OUTPUT);
445 |     pinMode(6, OUTPUT);
446 |     digitalWrite(5, HIGH);
447 |     digitalWrite(6, HIGH);
448 | 
449 |     // use PWM for microstepping support
450 |     // use PWM for microstepping support
451 |     initPWM3(STEPPER2_PWM_RATE);
452 |     initPWM4(STEPPER2_PWM_RATE);
453 |     setPWM3(255);
454 |     setPWM4(255);
455 |   }
456 | }
457 | 
458 | void AF_Stepper::setSpeed(uint16_t rpm) {
459 |   usperstep = 60000000 / ((uint32_t)revsteps * (uint32_t)rpm);
460 |   steppingcounter = 0;
461 | }
462 | 
463 | void AF_Stepper::release(void) {
464 |   if (steppernum == 1) {
465 |     latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) &
466 |       ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0
467 |     MC.latch_tx();
468 |   } else if (steppernum == 2) {
469 |     latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) &
470 |       ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0
471 |     MC.latch_tx();
472 |   }
473 | }
474 | 
475 | void AF_Stepper::step(uint16_t steps, uint8_t dir,  uint8_t style) {
476 |   uint32_t uspers = usperstep;
477 |   uint8_t ret = 0;
478 | 
479 |   if (style == INTERLEAVE) {
480 |     uspers /= 2;
481 |   }
482 |  else if (style == MICROSTEP) {
483 |     uspers /= MICROSTEPS;
484 |     steps *= MICROSTEPS;
485 | #ifdef MOTORDEBUG
486 |     Serial.print("steps = "); Serial.println(steps, DEC);
487 | #endif
488 |   }
489 | 
490 |   while (steps--) {
491 |     ret = onestep(dir, style);
492 |     delay(uspers/1000); // in ms
493 |     steppingcounter += (uspers % 1000);
494 |     if (steppingcounter >= 1000) {
495 |       delay(1);
496 |       steppingcounter -= 1000;
497 |     }
498 |   }
499 |   if (style == MICROSTEP) {
500 |     while ((ret != 0) && (ret != MICROSTEPS)) {
501 |       ret = onestep(dir, style);
502 |       delay(uspers/1000); // in ms
503 |       steppingcounter += (uspers % 1000);
504 |       if (steppingcounter >= 1000) {
505 | 	delay(1);
506 | 	steppingcounter -= 1000;
507 |       } 
508 |     }
509 |   }
510 | }
511 | 
512 | uint8_t AF_Stepper::onestep(uint8_t dir, uint8_t style) {
513 |   uint8_t a, b, c, d;
514 |   uint8_t ocrb, ocra;
515 | 
516 |   ocra = ocrb = 255;
517 | 
518 |   if (steppernum == 1) {
519 |     a = _BV(MOTOR1_A);
520 |     b = _BV(MOTOR2_A);
521 |     c = _BV(MOTOR1_B);
522 |     d = _BV(MOTOR2_B);
523 |   } else if (steppernum == 2) {
524 |     a = _BV(MOTOR3_A);
525 |     b = _BV(MOTOR4_A);
526 |     c = _BV(MOTOR3_B);
527 |     d = _BV(MOTOR4_B);
528 |   } else {
529 |     return 0;
530 |   }
531 | 
532 |   // next determine what sort of stepping procedure we're up to
533 |   if (style == SINGLE) {
534 |     if ((currentstep/(MICROSTEPS/2)) % 2) { // we're at an odd step, weird
535 |       if (dir == FORWARD) {
536 | 	currentstep += MICROSTEPS/2;
537 |       }
538 |       else {
539 | 	currentstep -= MICROSTEPS/2;
540 |       }
541 |     } else {           // go to the next even step
542 |       if (dir == FORWARD) {
543 | 	currentstep += MICROSTEPS;
544 |       }
545 |       else {
546 | 	currentstep -= MICROSTEPS;
547 |       }
548 |     }
549 |   } else if (style == DOUBLE) {
550 |     if (! (currentstep/(MICROSTEPS/2) % 2)) { // we're at an even step, weird
551 |       if (dir == FORWARD) {
552 | 	currentstep += MICROSTEPS/2;
553 |       } else {
554 | 	currentstep -= MICROSTEPS/2;
555 |       }
556 |     } else {           // go to the next odd step
557 |       if (dir == FORWARD) {
558 | 	currentstep += MICROSTEPS;
559 |       } else {
560 | 	currentstep -= MICROSTEPS;
561 |       }
562 |     }
563 |   } else if (style == INTERLEAVE) {
564 |     if (dir == FORWARD) {
565 |        currentstep += MICROSTEPS/2;
566 |     } else {
567 |        currentstep -= MICROSTEPS/2;
568 |     }
569 |   } 
570 | 
571 |   if (style == MICROSTEP) {
572 |     if (dir == FORWARD) {
573 |       currentstep++;
574 |     } else {
575 |       // BACKWARDS
576 |       currentstep--;
577 |     }
578 | 
579 |     currentstep += MICROSTEPS*4;
580 |     currentstep %= MICROSTEPS*4;
581 | 
582 |     ocra = ocrb = 0;
583 |     if ( (currentstep >= 0) && (currentstep < MICROSTEPS)) {
584 |       ocra = microstepcurve[MICROSTEPS - currentstep];
585 |       ocrb = microstepcurve[currentstep];
586 |     } else if  ( (currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) {
587 |       ocra = microstepcurve[currentstep - MICROSTEPS];
588 |       ocrb = microstepcurve[MICROSTEPS*2 - currentstep];
589 |     } else if  ( (currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) {
590 |       ocra = microstepcurve[MICROSTEPS*3 - currentstep];
591 |       ocrb = microstepcurve[currentstep - MICROSTEPS*2];
592 |     } else if  ( (currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) {
593 |       ocra = microstepcurve[currentstep - MICROSTEPS*3];
594 |       ocrb = microstepcurve[MICROSTEPS*4 - currentstep];
595 |     }
596 |   }
597 | 
598 |   currentstep += MICROSTEPS*4;
599 |   currentstep %= MICROSTEPS*4;
600 | 
601 | #ifdef MOTORDEBUG
602 |   Serial.print("current step: "); Serial.println(currentstep, DEC);
603 |   Serial.print(" pwmA = "); Serial.print(ocra, DEC); 
604 |   Serial.print(" pwmB = "); Serial.println(ocrb, DEC); 
605 | #endif
606 | 
607 |   if (steppernum == 1) {
608 |     setPWM1(ocra);
609 |     setPWM2(ocrb);
610 |   } else if (steppernum == 2) {
611 |     setPWM3(ocra);
612 |     setPWM4(ocrb);
613 |   }
614 | 
615 | 
616 |   // release all
617 |   latch_state &= ~a & ~b & ~c & ~d; // all motor pins to 0
618 | 
619 |   //Serial.println(step, DEC);
620 |   if (style == MICROSTEP) {
621 |     if ((currentstep >= 0) && (currentstep < MICROSTEPS))
622 |       latch_state |= a | b;
623 |     if ((currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2))
624 |       latch_state |= b | c;
625 |     if ((currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3))
626 |       latch_state |= c | d;
627 |     if ((currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4))
628 |       latch_state |= d | a;
629 |   } else {
630 |     switch (currentstep/(MICROSTEPS/2)) {
631 |     case 0:
632 |       latch_state |= a; // energize coil 1 only
633 |       break;
634 |     case 1:
635 |       latch_state |= a | b; // energize coil 1+2
636 |       break;
637 |     case 2:
638 |       latch_state |= b; // energize coil 2 only
639 |       break;
640 |     case 3:
641 |       latch_state |= b | c; // energize coil 2+3
642 |       break;
643 |     case 4:
644 |       latch_state |= c; // energize coil 3 only
645 |       break; 
646 |     case 5:
647 |       latch_state |= c | d; // energize coil 3+4
648 |       break;
649 |     case 6:
650 |       latch_state |= d; // energize coil 4 only
651 |       break;
652 |     case 7:
653 |       latch_state |= d | a; // energize coil 1+4
654 |       break;
655 |     }
656 |   }
657 | 
658 |  
659 |   MC.latch_tx();
660 |   return currentstep;
661 | }
662 | 
663 | 


--------------------------------------------------------------------------------
/AFMotor.h:
--------------------------------------------------------------------------------
  1 | #ifndef _AFMotor_h_
  2 | #define _AFMotor_h_
  3 | 
  4 | #include <inttypes.h>
  5 | #if defined(__AVR__)
  6 |     #include <avr/io.h>
  7 | 
  8 |     //#define MOTORDEBUG 1
  9 | 
 10 |     #define MICROSTEPS 16                       // 8 or 16
 11 | 
 12 |     #define MOTOR12_64KHZ _BV(CS20)             // no prescale
 13 |     #define MOTOR12_8KHZ _BV(CS21)              // divide by 8
 14 |     #define MOTOR12_2KHZ _BV(CS21) | _BV(CS20)  // divide by 32
 15 |     #define MOTOR12_1KHZ _BV(CS22)              // divide by 64
 16 | 
 17 |     #define MOTOR34_64KHZ _BV(CS00)             // no prescale
 18 |     #define MOTOR34_8KHZ _BV(CS01)              // divide by 8
 19 |     #define MOTOR34_1KHZ _BV(CS01) | _BV(CS00)  // divide by 64
 20 |     
 21 |     #define DC_MOTOR_PWM_RATE   MOTOR34_8KHZ    // PWM rate for DC motors
 22 |     #define STEPPER1_PWM_RATE   MOTOR12_64KHZ   // PWM rate for stepper 1
 23 |     #define STEPPER2_PWM_RATE   MOTOR34_64KHZ   // PWM rate for stepper 2
 24 |     
 25 | #elif defined(__PIC32MX__)
 26 |     //#define MOTORDEBUG 1
 27 |     
 28 |     // Uncomment the one of following lines if you have put a jumper from 
 29 |     // either pin 9 to pin 11 or pin 10 to pin 11 on your Motor Shield.
 30 |     // Either will enable PWM for M1
 31 |     //#define PIC32_USE_PIN9_FOR_M1_PWM
 32 |     //#define PIC32_USE_PIN10_FOR_M1_PWM
 33 | 
 34 |     #define MICROSTEPS 16       // 8 or 16
 35 | 
 36 |     // For PIC32 Timers, define prescale settings by PWM frequency
 37 |     #define MOTOR12_312KHZ  0   // 1:1, actual frequency 312KHz
 38 |     #define MOTOR12_156KHZ  1   // 1:2, actual frequency 156KHz
 39 |     #define MOTOR12_64KHZ   2   // 1:4, actual frequency 78KHz
 40 |     #define MOTOR12_39KHZ   3   // 1:8, acutal frequency 39KHz
 41 |     #define MOTOR12_19KHZ   4   // 1:16, actual frequency 19KHz
 42 |     #define MOTOR12_8KHZ    5   // 1:32, actual frequency 9.7KHz
 43 |     #define MOTOR12_4_8KHZ  6   // 1:64, actual frequency 4.8KHz
 44 |     #define MOTOR12_2KHZ    7   // 1:256, actual frequency 1.2KHz
 45 |     #define MOTOR12_1KHZ    7   // 1:256, actual frequency 1.2KHz
 46 | 
 47 |     #define MOTOR34_312KHZ  0   // 1:1, actual frequency 312KHz
 48 |     #define MOTOR34_156KHZ  1   // 1:2, actual frequency 156KHz
 49 |     #define MOTOR34_64KHZ   2   // 1:4, actual frequency 78KHz
 50 |     #define MOTOR34_39KHZ   3   // 1:8, acutal frequency 39KHz
 51 |     #define MOTOR34_19KHZ   4   // 1:16, actual frequency 19KHz
 52 |     #define MOTOR34_8KHZ    5   // 1:32, actual frequency 9.7KHz
 53 |     #define MOTOR34_4_8KHZ  6   // 1:64, actual frequency 4.8KHz
 54 |     #define MOTOR34_2KHZ    7   // 1:256, actual frequency 1.2KHz
 55 |     #define MOTOR34_1KHZ    7   // 1:256, actual frequency 1.2KHz
 56 |     
 57 |     // PWM rate for DC motors.
 58 |     #define DC_MOTOR_PWM_RATE   MOTOR34_39KHZ
 59 |     // Note: for PIC32, both of these must be set to the same value
 60 |     // since there's only one timebase for all 4 PWM outputs
 61 |     #define STEPPER1_PWM_RATE   MOTOR12_39KHZ
 62 |     #define STEPPER2_PWM_RATE   MOTOR34_39KHZ
 63 |     
 64 | #endif
 65 | 
 66 | // Bit positions in the 74HCT595 shift register output
 67 | #define MOTOR1_A 2
 68 | #define MOTOR1_B 3
 69 | #define MOTOR2_A 1
 70 | #define MOTOR2_B 4
 71 | #define MOTOR4_A 0
 72 | #define MOTOR4_B 6
 73 | #define MOTOR3_A 5
 74 | #define MOTOR3_B 7
 75 | 
 76 | // Constants that the user passes in to the motor calls
 77 | #define FORWARD 1
 78 | #define BACKWARD 2
 79 | #define BRAKE 3
 80 | #define RELEASE 4
 81 | 
 82 | // Constants that the user passes in to the stepper calls
 83 | #define SINGLE 1
 84 | #define DOUBLE 2
 85 | #define INTERLEAVE 3
 86 | #define MICROSTEP 4
 87 | 
 88 | /*
 89 | #define LATCH 4
 90 | #define LATCH_DDR DDRB
 91 | #define LATCH_PORT PORTB
 92 | 
 93 | #define CLK_PORT PORTD
 94 | #define CLK_DDR DDRD
 95 | #define CLK 4
 96 | 
 97 | #define ENABLE_PORT PORTD
 98 | #define ENABLE_DDR DDRD
 99 | #define ENABLE 7
100 | 
101 | #define SER 0
102 | #define SER_DDR DDRB
103 | #define SER_PORT PORTB
104 | */
105 | 
106 | // Arduino pin names for interface to 74HCT595 latch
107 | #define MOTORLATCH 12
108 | #define MOTORCLK 4
109 | #define MOTORENABLE 7
110 | #define MOTORDATA 8
111 | 
112 | class AFMotorController
113 | {
114 |   public:
115 |     AFMotorController(void);
116 |     void enable(void);
117 |     friend class AF_DCMotor;
118 |     void latch_tx(void);
119 |     uint8_t TimerInitalized;
120 | };
121 | 
122 | class AF_DCMotor
123 | {
124 |  public:
125 |   AF_DCMotor(uint8_t motornum, uint8_t freq = DC_MOTOR_PWM_RATE);
126 |   void run(uint8_t);
127 |   void setSpeed(uint8_t);
128 | 
129 |  private:
130 |   uint8_t motornum, pwmfreq;
131 | };
132 | 
133 | class AF_Stepper {
134 |  public:
135 |   AF_Stepper(uint16_t, uint8_t);
136 |   void step(uint16_t steps, uint8_t dir,  uint8_t style = SINGLE);
137 |   void setSpeed(uint16_t);
138 |   uint8_t onestep(uint8_t dir, uint8_t style);
139 |   void release(void);
140 |   uint16_t revsteps; // # steps per revolution
141 |   uint8_t steppernum;
142 |   uint32_t usperstep, steppingcounter;
143 |  private:
144 |   uint8_t currentstep;
145 | 
146 | };
147 | 
148 | uint8_t getlatchstate(void);
149 | 
150 | #endif
151 | 


--------------------------------------------------------------------------------
/Obstacle Avoiding Robot using Arduino and L293d.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embeddedlab786/Obstacle_Avoiding_Robot/e40fa0be6fc6bc205497c8b3e609900ef2d843c2/Obstacle Avoiding Robot using Arduino and L293d.jpg


--------------------------------------------------------------------------------
/Obstacle_Avoiding_Robot.ino:
--------------------------------------------------------------------------------
  1 | #include "AFMotor.h"
  2 | #include <Servo.h>
  3 | 
  4 | #define echopin A4 // echo pin
  5 | #define trigpin A5 // Trigger pin
  6 | 
  7 | Servo myservo;
  8 | 
  9 | const int MOTOR_1 = 1; 
 10 | const int MOTOR_2 = 2; 
 11 | const int MOTOR_3 = 3; 
 12 | const int MOTOR_4 = 4; 
 13 | 
 14 | AF_DCMotor motor1(MOTOR_1, MOTOR12_64KHZ); // create motor object, 64KHz pwm
 15 | AF_DCMotor motor2(MOTOR_2, MOTOR12_64KHZ); // create motor object, 64KHz pwm
 16 | AF_DCMotor motor3(MOTOR_3, MOTOR12_64KHZ); // create motor object, 64KHz pwm
 17 | AF_DCMotor motor4(MOTOR_4, MOTOR12_64KHZ); // create motor object, 64KHz pwm
 18 | //===============================================================================
 19 | //  Initialization
 20 | //===============================================================================
 21 | 
 22 | int distance_L, distance_F, distance_R;
 23 | long distance;
 24 | 
 25 | int set = 20;
 26 |  
 27 | void setup() {
 28 |   Serial.begin(9600);           // Initialize serial port
 29 |   Serial.println("Start");
 30 | 
 31 |   myservo.attach(10);
 32 |   myservo.write(90);
 33 | 
 34 |   pinMode (trigpin, OUTPUT);
 35 |   pinMode (echopin, INPUT );
 36 |   
 37 |   motor1.setSpeed(180);          // set the motor speed to 0-255
 38 |   motor2.setSpeed(180);
 39 |   motor3.setSpeed(180);
 40 |   motor4.setSpeed(180);
 41 | }
 42 | //===============================================================================
 43 | //  Main
 44 | //=============================================================================== 
 45 | void loop() {
 46 |  distance_F = data();
 47 |  Serial.print("S=");
 48 |  Serial.println(distance_F);
 49 |   if (distance_F > set){
 50 |    Serial.println("Forward");
 51 |   motor1.run(FORWARD);         // turn it on going forward
 52 |   motor2.run(FORWARD); 
 53 |   motor3.run(FORWARD); 
 54 |   motor4.run(FORWARD);
 55 |     }
 56 |     else{hc_sr4();}
 57 | }
 58 | 
 59 | 
 60 | long data(){
 61 |   digitalWrite(trigpin, LOW);
 62 |   delayMicroseconds(2);
 63 |   digitalWrite(trigpin, HIGH);
 64 |   delayMicroseconds(10);
 65 |   distance = pulseIn (echopin, HIGH);
 66 |   return distance / 29 / 2;
 67 | }
 68 | 
 69 | 
 70 | void compareDistance(){
 71 |   if (distance_L > distance_R){
 72 |   motor1.run(BACKWARD);   // turn it on going left
 73 |   motor2.run(BACKWARD);
 74 |   motor3.run(FORWARD); 
 75 |   motor4.run(FORWARD); 
 76 |     delay(350);
 77 |   }
 78 |   else if (distance_R > distance_L){
 79 |   motor1.run(FORWARD);  // the other right
 80 |   motor2.run(FORWARD); 
 81 |   motor3.run(BACKWARD); 
 82 |   motor4.run(BACKWARD);
 83 |     delay(350);
 84 |   }
 85 |   else{
 86 |   motor1.run(BACKWARD);  // the other way
 87 |   motor2.run(BACKWARD);
 88 |   motor3.run(BACKWARD); 
 89 |   motor4.run(BACKWARD); 
 90 |    delay(300);
 91 |   motor1.run(BACKWARD);   // turn it on going left
 92 |   motor2.run(BACKWARD);
 93 |   motor3.run(FORWARD); 
 94 |   motor4.run(FORWARD); 
 95 |     delay(500);
 96 |   }
 97 | }
 98 | 
 99 | void hc_sr4(){
100 |     Serial.println("Stop");
101 |     motor1.run(RELEASE);         // stopped
102 |     motor2.run(RELEASE);
103 |     motor3.run(RELEASE);
104 |     motor4.run(RELEASE);
105 |    
106 |     myservo.write(0);
107 |     delay(300);
108 |     distance_R = data();
109 |     delay(100);
110 |     myservo.write(170);
111 |     delay(500);
112 |     distance_L = data();
113 |     delay(100);
114 |     myservo.write(90);
115 |     delay(300);
116 |     compareDistance();
117 | }
118 | 
119 | 
120 | 
121 | 
122 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # Obstacle_Avoiding_Robot
2 | Obstacle_Avoiding_Robot
3 | ![Obstacle Avoiding Robot using Arduino and L293d](https://user-images.githubusercontent.com/57707946/73702842-cf867b00-470f-11ea-8dd8-f32f9d5bfc0a.jpg)
4 | ![obstacle avoiding robot using arduino uno and L293d with hc-sr04 sensor](https://user-images.githubusercontent.com/57707946/73702855-ddd49700-470f-11ea-833a-8912602be1a4.jpg)
5 | 


--------------------------------------------------------------------------------