Advertisement
Aminpro

Arduino Cybot Light Follower[Test Code 3]

Feb 17th, 2013
264
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 1.56 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 < 200){
  36.    
  37.  
  38.  
  39.   analogWrite(rightfor, 225);
  40.   analogWrite(rightback, 0);
  41.   analogWrite(leftfor, 225);
  42.   analogWrite(leftback, 0);
  43.  
  44.   diff = analogRead(A0) - analogRead(A1);
  45.   if(diff < 0) diff = (-1) * diff;
  46.  
  47.   }
  48.  
  49.   correction(diff);
  50.  
  51.   if ( analogRead(A0) > analogRead(A1)){
  52.  
  53.   analogWrite(rightfor, 20);
  54.   analogWrite(rightback, 0);
  55.   analogWrite(leftfor, correctionspeed);
  56.   analogWrite(leftback, 0);
  57.  
  58.  
  59.   }
  60.   else if ( analogRead(A1) > analogRead(A0)){
  61.  
  62.     analogWrite(rightfor, correctionspeed);
  63.     analogWrite(rightback, 0);
  64.     analogWrite(leftfor, 20);
  65.     analogWrite(leftback, 0);
  66.    
  67.  
  68.   }
  69.   }
  70.   else {
  71.    
  72.     analogWrite(rightfor, 0);
  73.     analogWrite(rightback, 0);
  74.     analogWrite(leftfor, 0);
  75.     analogWrite(leftback, 0);
  76.  
  77.   }
  78.   Serial.println("");
  79.   Serial.println(analogRead(A0));
  80.   Serial.println(analogRead(A1));
  81.   Serial.println(analogRead(A0)-analogRead(A1));
  82.   delay(100);
  83.  
  84. }
  85.  
  86. int correction(int diff){
  87.  
  88.   correctionspeed = (diff * (225/400)) + 127;
  89.    
  90. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement