Advertisement
Aminpro

Arduino Cybot Light Follower[Test Code 4]

Feb 18th, 2013
566
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 1.59 KB | None | 0 0
  1. //http://www.youtube.com/watch?v=w7Sdtg9mHGM
  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. void setup(){
  16.  
  17.   Serial.begin(9600);
  18.  
  19.   pinMode(A0, INPUT);
  20.   pinMode(A1, INPUT);
  21.   pinMode(rightfor, OUTPUT);
  22.   pinMode(rightback, OUTPUT);
  23.   pinMode(leftfor, OUTPUT);
  24.   pinMode(leftback, OUTPUT);
  25.  
  26. }
  27.  
  28. void loop(){
  29.  
  30.   int leftlight = analogRead(A1);
  31.   int rightlight = analogRead(A0);
  32.  
  33.   leftlight = map(leftlight, 0, 950, 0, 255);
  34.   rightlight = map(rightlight, 0, 890, 0, 255);
  35.  
  36.   leftlight = constrain(leftlight, 0, 255);
  37.   rightlight = constrain(rightlight, 0, 255);
  38.  
  39.   if( analogRead(A0) > 26 && analogRead(A1) > 26){
  40.    
  41.   diff = leftlight - rightlight;
  42.   if(diff < 0) diff = (-1) * diff;
  43.   correctionspeed = diff + 170;
  44.   correctionspeed = constrain(correctionspeed, 0, 255);
  45.  
  46.   if(diff < 26 ){
  47.  
  48.   analogWrite(rightfor, 225);
  49.   analogWrite(rightback, 0);
  50.   analogWrite(leftfor, 225);
  51.   analogWrite(leftback, 0);
  52.  
  53.   }
  54.  
  55.   if ( leftlight > rightlight){
  56.  
  57.   analogWrite(rightfor, correctionspeed);
  58.   analogWrite(rightback, 0);
  59.   analogWrite(leftfor, 0);
  60.   analogWrite(leftback, 0);
  61.  
  62.  
  63.  
  64.   }
  65.   else if ( rightlight > leftlight){
  66.  
  67.     analogWrite(rightfor, 0);
  68.     analogWrite(rightback, 0);
  69.     analogWrite(leftfor,  correctionspeed);
  70.     analogWrite(leftback, 0);
  71.   }
  72.  
  73.  
  74.   }
  75.   else {
  76.    
  77.     analogWrite(rightfor, 0);
  78.     analogWrite(rightback, 0);
  79.     analogWrite(leftfor, 0);
  80.     analogWrite(leftback, 0);
  81.  
  82.  
  83.  
  84.   }
  85. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement