Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- http://youtu.be/lrTrVeVSFV4
- //Motor control pins
- int rightfor = 11;
- int rightback = 10;
- int leftfor = 9;
- int leftback = 6;
- //Variables
- int diff = 0;
- int correctionspeed = 0;
- //Prototypes
- int correction(int diff);
- void setup(){
- Serial.begin(9600);
- pinMode(A0, INPUT);
- pinMode(A1, INPUT);
- pinMode(rightfor, OUTPUT);
- pinMode(rightback, OUTPUT);
- pinMode(leftfor, OUTPUT);
- pinMode(leftback, OUTPUT);
- }
- void loop(){
- if( analogRead(A0) > 10 || analogRead(A1) > 10){
- while(diff < 200){
- analogWrite(rightfor, 225);
- analogWrite(rightback, 0);
- analogWrite(leftfor, 225);
- analogWrite(leftback, 0);
- diff = analogRead(A0) - analogRead(A1);
- if(diff < 0) diff = (-1) * diff;
- }
- correction(diff);
- if ( analogRead(A0) > analogRead(A1)){
- analogWrite(rightfor, 20);
- analogWrite(rightback, 0);
- analogWrite(leftfor, correctionspeed);
- analogWrite(leftback, 0);
- }
- else if ( analogRead(A1) > analogRead(A0)){
- analogWrite(rightfor, correctionspeed);
- analogWrite(rightback, 0);
- analogWrite(leftfor, 20);
- analogWrite(leftback, 0);
- }
- }
- else {
- analogWrite(rightfor, 0);
- analogWrite(rightback, 0);
- analogWrite(leftfor, 0);
- analogWrite(leftback, 0);
- }
- Serial.println("");
- Serial.println(analogRead(A0));
- Serial.println(analogRead(A1));
- Serial.println(analogRead(A0)-analogRead(A1));
- delay(100);
- }
- int correction(int diff){
- correctionspeed = (diff * (225/400)) + 127;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement