Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <RedBot.h> //includes the redbot library
- RedBotMotors motors;// Instantiate the motor control object.
- RedBotEncoder encoder = RedBotEncoder(A3, 10); // Initialize encoders
- //1 wheel turn goes 20.42 centimeters
- //one meter is 4.897 turns
- //one wheel turn is 48 motor turns
- //one meter is 235 motor turns
- //one meter is 940 encoder ticks
- //one 90* turn is 66 motor turns.
- int lCount=0;
- int rCount=0;
- int n = 0;
- bool amIStraight = true;
- void setup(){
- Serial.begin(9600);
- Serial.println("left right");
- Serial.println("================");
- encoder.clearEnc(BOTH);
- }
- void loop() {
- lCount = encoder.getTicks(LEFT); // read the left motor encoder
- rCount = encoder.getTicks(RIGHT);
- Serial.print(lCount);
- Serial.print(" ");
- Serial.print("\t");
- Serial.println(rCount);
- while(amIStraight==true){
- lCount = encoder.getTicks(LEFT); // read the left motor encoder
- rCount = encoder.getTicks(RIGHT);
- Serial.print(lCount);
- Serial.print(" ");
- Serial.print("\t");
- Serial.println(rCount);
- motors.leftMotor(-60);
- motors.rightMotor(56);
- if(lCount > rCount + 10){
- motors.leftMotor(-57);
- motors.rightMotor(56);
- }
- if(lCount + 10 < rCount) {
- motors.rightMotor(54);
- motors.leftMotor(-60);
- }
- if((lCount + 10 > rCount) && (lCount < rCount + 10)){
- motors.leftMotor(-60);
- motors.rightMotor(57);
- }
- if(lCount > 940 && rCount > 940){
- amIStraight = false;
- motors.stop();
- encoder.clearEnc(BOTH);
- }
- }
- //This is where I come to cry. GO ON HOUR 5 BABY!
- delay(500);
- while(amIStraight == false){
- lCount = encoder.getTicks(LEFT); // read the left motor encoder
- rCount = encoder.getTicks(RIGHT);
- Serial.print(lCount);
- Serial.print(" ");
- Serial.print("\t");
- Serial.println(rCount);
- motors.leftMotor(-60);
- motors.rightMotor(0);
- if(lCount > 204){
- motors.stop();
- amIStraight = true;
- encoder.clearEnc(BOTH);
- }
- delay(500);
- }}
Advertisement
Add Comment
Please, Sign In to add comment