Advertisement
Aminpro

Arduino Cybot Light Follower[Prototype Code]

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