Advertisement
Aminpro

Arduino Cybot Light Follower[Test Code 1]

Feb 14th, 2013
392
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 1.41 KB | None | 0 0
  1. //http://www.youtube.com/watch?v=gqFn7G5TBcE
  2.  
  3. //Motor control pins
  4.  
  5. int rightfor = 11;
  6. int rightback = 10;
  7. int leftfor = 9;
  8. int leftback = 6;
  9.  
  10. //Variables
  11.  
  12. int diff = 0;
  13. int correctionspeed = 0;
  14.  
  15. //Prototypes
  16. int correction(int diff);
  17.  
  18.  
  19. void setup(){
  20.  
  21.   pinMode(A0, INPUT);
  22.   pinMode(A1, INPUT);
  23.   pinMode(rightfor, OUTPUT);
  24.   pinMode(rightback, OUTPUT);
  25.   pinMode(leftfor, OUTPUT);
  26.   pinMode(leftback, OUTPUT);
  27.  
  28. }
  29.  
  30. void loop(){
  31.  
  32.   if( analogRead(A0) > 10 || analogRead(A1) > 10){
  33.   while(diff < 10 ){
  34.  
  35.   analogWrite(rightfor, 225);
  36.   analogWrite(rightback, 0);
  37.   analogWrite(leftfor, 225);
  38.   analogWrite(leftback, 0);
  39.  
  40.   diff = analogRead(A0) - analogRead(A1);
  41.   if(diff < 0) diff = (-1) * diff;
  42.  
  43.   }
  44.  
  45.   correction(diff);
  46.  
  47.   if ( analogRead(A0) > analogRead(A1)){
  48.  
  49.   analogWrite(rightfor, 0);
  50.   analogWrite(rightback, 0);
  51.   analogWrite(leftfor, correctionspeed);
  52.   analogWrite(leftback, 0);
  53.   delay(100);
  54.  
  55.   }
  56.   else if ( analogRead(A1) > analogRead(A0)){
  57.  
  58.     analogWrite(rightfor, correctionspeed);
  59.     analogWrite(rightback, 0);
  60.     analogWrite(leftfor, 0);
  61.     analogWrite(leftback, 0);
  62.     delay(100);
  63.  
  64.   }
  65.   }
  66.   else {
  67.    
  68.     analogWrite(rightfor, 0);
  69.     analogWrite(rightback, 0);
  70.     analogWrite(leftfor, 0);
  71.     analogWrite(leftback, 0);
  72.  
  73.   }
  74.  
  75. }
  76.  
  77. int correction(int diff){
  78.  
  79.   correctionspeed = (diff * (225/400)) + 127;
  80.    
  81. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement