File content as of revision 15:b80555a4a8b9:

//void turnLeft(){
//    double speed0 = 0.15;
//    double speed1 = 0.15;
//    double kp = 0.01;
//
//    int counter = 0;
//    
//    int initialCount0 = encoder0.getPulses();
//    int initialCount1 = encoder1.getPulses();
//
//    double desiredCount0 = initialCount0 - desiredCountL;
//    double desiredCount1 = initialCount1 + desiredCountL;
//
//    int count0 = initialCount0;
//    int count1 = initialCount1;
//
//    double error0 = count0 - desiredCount0;
//    double error1 = count1 - desiredCount1;
//
//    while(1){
//
//        if(!(abs(error0) < 1) && !(abs(error1) < 1)){
//            count0 = encoder0.getPulses();
//            count1 = encoder1.getPulses();
//
//            error0 = count0 - desiredCount0; 
//            error1 = count1 - desiredCount1; 
//
//            speed0 = error0 * kp + speed0;
//            speed1 = error1 * kp + speed1;
//
//            right_motor.move(speed0);
//            left_motor.move(speed1);
//            counter = 0;
//        }else{
//            counter++;
//            right_motor.brake();
//            left_motor.brake();
//        }
//
//        if (counter > 60){
//            break;
//        }
//     }
//
//    right_motor.brake();
//    left_motor.brake();
//}
//
//void turnRight() { //e1 should be negative encoder0 is left, encoder1 is right
//    double speed0 = 0.15;
//    double speed1 = 0.15;
//    double kp = 0.01;
//
//    int counter = 0;
//    
//    int initialCount0 = encoder0.getPulses();
//    int initialCount1 = encoder1.getPulses();
//
//    double desiredCount0 = initialCount0 + desiredCountR;
//    double desiredCount1 = initialCount1 - desiredCountR;
//
//    int count0 = initialCount0;
//    int count1 = initialCount1;
//
//    double error0 = count0 - desiredCount0;
//    double error1 = count1 - desiredCount1;
//
//    while(1){
//
//        if(!(abs(error0) < 1) && !(abs(error1) < 1)){
//            count0 = encoder0.getPulses();
//            count1 = encoder1.getPulses();
//
//            error0 = count0 - desiredCount0; // moves forward
//            error1 = count1 - desiredCount1; // moves backwards
//
//            speed0 = error0 * kp + speed0;
//            speed1 = error1 * kp + speed1;
//
//            right_motor.move(speed0);
//            left_motor.move(speed1);
//            counter = 0;
//        }else{
//            counter++;
//            right_motor.brake();
//            left_motor.brake();
//        }
//
//        if (counter > 60){
//            break;
//        }
//        
//        // serial.printf("ERROR=> 0:%f, 1:%f, SPEED=> 0:%f, 1:%f\n", error0, error1, speed0, speed1);
//        // serial.printf("Pulse Count=> e0:%d, e1:%d      \r\n", encoder0.getPulses(), encoder1.getPulses());
//    }
//
//    right_motor.brake();
//    left_motor.brake();
//}
//
//
//void turnLeft180(){
//    double speed0 = 0.15;
//    double speed1 = 0.15;
//    double kp = 0.01;
//
//    int counter = 0;
//    
//    int initialCount0 = encoder0.getPulses();
//    int initialCount1 = encoder1.getPulses();
//
//    double desiredCount0 = initialCount0 - 3000;
//    double desiredCount1 = initialCount1 + 2700;
//
//    int count0 = initialCount0;
//    int count1 = initialCount1;
//
//    double error0 = count0 - desiredCount0;
//    double error1 = count1 - desiredCount1;
//
//    while(1){
//
//        if(!(abs(error0) < 1) && !(abs(error1) < 1)){
//            count0 = encoder0.getPulses();
//            count1 = encoder1.getPulses();
//
//            error0 = count0 - desiredCount0; 
//            error1 = count1 - desiredCount1; 
//
//            speed0 = error0 * kp + speed0;
//            speed1 = error1 * kp + speed1;
//
//            right_motor.move(speed0);
//            left_motor.move(speed1);
//            counter = 0;
//        }else{
//            counter++;
//            right_motor.brake();
//            left_motor.brake();
//        }
//
//        if (counter > 60){
//            break;
//        }
//     }
//
//    right_motor.brake();
//    left_motor.brake();
//}
//
//void turnRight180(){
//    double speed0 = 0.15;
//    double speed1 = 0.15;
//    double kp = 0.01;
//
//    int counter = 0;
//    
//    int initialCount0 = encoder0.getPulses();
//    int initialCount1 = encoder1.getPulses();
//
//    double desiredCount0 = initialCount0 + desiredCountR*2;
//    double desiredCount1 = initialCount1 - desiredCountR*2;
//
//    int count0 = initialCount0;
//    int count1 = initialCount1;
//
//    double error0 = count0 - desiredCount0;
//    double error1 = count1 - desiredCount1;
//
//    while(1){
//
//        if(!(abs(error0) < 1) && !(abs(error1) < 1)){
//            count0 = encoder0.getPulses();
//            count1 = encoder1.getPulses();
//
//            error0 = count0 - desiredCount0; // moves forward
//            error1 = count1 - desiredCount1; // moves backwards
//
//            speed0 = error0 * kp + speed0;
//            speed1 = error1 * kp + speed1;
//
//            right_motor.move(speed0);
//            left_motor.move(speed1);
//            counter = 0;
//        }else{
//            counter++;
//            right_motor.brake();
//            left_motor.brake();
//        }
//
//        if (counter > 60){
//            break;
//        }
//        
//        // serial.printf("ERROR=> 0:%f, 1:%f, SPEED=> 0:%f, 1:%f\n", error0, error1, speed0, speed1);
//        // serial.printf("Pulse Count=> e0:%d, e1:%d      \r\n", encoder0.getPulses(), encoder1.getPulses());
//    }
//
//    right_motor.brake();
//    left_motor.brake();
//}