Advertisement
Aminpro

Arduino Cybot Light Follower[Test Code 2]

Feb 16th, 2013
380
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 1.51 KB | None | 0 0
  1. http://youtu.be/lrTrVeVSFV4
  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.   Serial.begin(9600);
  22.  
  23.   pinMode(A0, INPUT);
  24.   pinMode(A1, INPUT);
  25.   pinMode(rightfor, OUTPUT);
  26.   pinMode(rightback, OUTPUT);
  27.   pinMode(leftfor, OUTPUT);
  28.   pinMode(leftback, OUTPUT);
  29.  
  30. }
  31.  
  32. void loop(){
  33.  
  34.   if( analogRead(A0) > 10 || analogRead(A1) > 10){
  35.   while(diff < 10 ){
  36.  
  37.   analogWrite(rightfor, 225);
  38.   analogWrite(rightback, 0);
  39.   analogWrite(leftfor, 225);
  40.   analogWrite(leftback, 0);
  41.  
  42.   diff = analogRead(A0) - analogRead(A1);
  43.   if(diff < 0) diff = (-1) * diff;
  44.  
  45.   }
  46.  
  47.   correction(diff);
  48.  
  49.   if ( analogRead(A0) > analogRead(A1)){
  50.  
  51.   analogWrite(rightfor, 0);
  52.   analogWrite(rightback, 0);
  53.   analogWrite(leftfor, correctionspeed);
  54.   analogWrite(leftback, 0);
  55.   delay(100);
  56.  
  57.   }
  58.   else if ( analogRead(A1) > analogRead(A0)){
  59.  
  60.     analogWrite(rightfor, correctionspeed);
  61.     analogWrite(rightback, 0);
  62.     analogWrite(leftfor, 0);
  63.     analogWrite(leftback, 0);
  64.     delay(100);
  65.  
  66.   }
  67.   }
  68.   else {
  69.    
  70.     analogWrite(rightfor, 0);
  71.     analogWrite(rightback, 0);
  72.     analogWrite(leftfor, 0);
  73.     analogWrite(leftback, 0);
  74.  
  75.   }
  76.   Serial.println("");
  77.   Serial.println(analogRead(A0));
  78.   Serial.println(analogRead(A1));
  79.  
  80. }
  81.  
  82. int correction(int diff){
  83.  
  84.   correctionspeed = (diff * (225/400)) + 127;
  85.    
  86. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement