Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

Files at this revision

API Documentation at this revision

Comitter:
christine222
Date:
Thu May 18 02:52:22 2017 +0000
Parent:
20:82836745332e
Child:
22:681190ff98f0
Commit message:
better PID constants for forwardWallIR and added no wall encoder pid moveForwardEncoder() function that is used in forwardWallIR()

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/main.cpp	Wed May 17 02:01:34 2017 +0000
+++ b/main.cpp	Thu May 18 02:52:22 2017 +0000
@@ -16,9 +16,12 @@
 */
 
 // Constants for when HIGH_PWM_VOLTAGE = 0.1
-#define IP_CONSTANT 8.85
-#define II_CONSTANT 0.005
-#define ID_CONSTANT 3.15
+// #define IP_CONSTANT 8.85
+// #define II_CONSTANT 0.005
+// #define ID_CONSTANT 3.15
+#define IP_CONSTANT 7.85
+#define II_CONSTANT 0.095
+#define ID_CONSTANT 20.85
 
 const int desiredCountR = 1400;
 const int desiredCountL = 1475;
@@ -33,6 +36,7 @@
 
 void pidOnEncoders();
 
+
 void turnLeft()
 {
     double speed0 = 0.15;
@@ -265,6 +269,112 @@
     }
 }
 
+void pidBrake(){
+
+    int count0;
+    int count1;
+    count0 = encoder0.getPulses();
+    count1 = encoder1.getPulses();
+    int initial0 = count0;
+    int initial1 = count1;
+    double kp = 0.00011;
+
+
+
+    int error = count0 - count1;
+
+    int counter = 0;
+    right_motor.move(0.7);
+    left_motor.move(0.7);
+
+    double speed0 = 0.7;
+    double speed1 = 0.7;
+
+    while(1)
+    {
+        if(abs(error) < 3){
+            right_motor.brake();
+            left_motor.brake();
+            counter++;
+        }else{
+            count0 = encoder0.getPulses() - initial0;
+            count1 = encoder1.getPulses() - initial1;
+            error = count0 - count1;
+            speed0 = -error*kp + speed0;
+            speed1 = error*kp + speed1;
+
+            right_motor.move(speed0);
+            left_motor.move(speed1);
+
+            counter = 0;
+        }
+        if (counter > 10){
+            break;
+        }
+
+    }
+    return;
+}
+
+void moveForwardEncoder(){
+
+    int count0;
+    int count1;
+    count0 = encoder0.getPulses();
+    count1 = encoder1.getPulses();
+    int initial1 = count1;
+    int initial0 = count0;
+    int diff = count0 - count1;
+    double kp = 0.00015;
+    double kd = 0.00019;
+    int prev = 0;
+
+
+
+    double speed0 = 0.1;
+    double speed1 = 0.13;
+    right_motor.move(speed0);
+    left_motor.move(speed1);
+
+
+    while((IRP_2.getSamples(SAMPLE_NUM) < 0.005 || IRP_3.getSamples(SAMPLE_NUM) < 0.005) && ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.25) ) {
+
+        //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
+  
+        count0 = encoder0.getPulses() - initial0;
+        count1 = encoder1.getPulses() - initial1;
+        int x = count0 - count1;
+        //double d = kp * x + kd * ( x - prev );
+        double kppart = kp * x;
+        double kdpart = kd * (x-prev);
+        double d = kppart + kdpart;
+        
+        //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart );
+        if( x < diff - 40 ) // count1 is bigger, right wheel pushed forward
+        {
+            left_motor.move( speed1-0.8*d );
+            right_motor.move( speed0+d );
+        }
+        else if( x > diff + 40 )
+        {
+            left_motor.move( speed1-0.8*d );
+            right_motor.move( speed0+d );
+        }
+        // else
+        // {
+        //     left_motor.brake();
+        //     right_motor.brake();   
+        // }
+        prev = x; 
+    }
+
+    //pidOnEncoders();
+    pidBrake();
+    //right_motor.brake();
+    //left_motor.brake();
+    return;
+}
+
 void moveForwardUntilWallIr()
 {
     double currentError = 0;
@@ -274,53 +384,62 @@
 
     double HIGH_PWM_VOLTAGE = 0.1;
 
-    double rightSpeed = 0.1;
-    double leftSpeed = 0.12;
+    double rightSpeed = 0.14;
+    double leftSpeed = 0.17;
 
     float ir2 = IRP_2.getSamples( SAMPLE_NUM );
     float ir3 = IRP_3.getSamples( SAMPLE_NUM );
 
     int count = encoder0.getPulses();
-    while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) {
-        int pulseCount = (encoder0.getPulses()-count) % 5600;
-        if (pulseCount > 5400 && pulseCount < 5800) {
-            blueLed.write(0);
-        } else {
-            blueLed.write(1);
-        }
-        sumError += currentError;
-        currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
-        derivError = currentError - previousError;
-        double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
-        if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
-            rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
-            leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
-        } else { // r is faster than L. speed up l, slow down r
-            rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
-            leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
+    while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) { // while the front facing IR's arent covered
+        
+        if((IRP_2.getSamples(SAMPLE_NUM) < 0.005 || IRP_3.getSamples(SAMPLE_NUM) < 0.005)) {
+            serial.printf("asdfasdf \n");
+            moveForwardEncoder(); 
+        }else{
+        // will move forward using encoders only 
+        // if cell ahead doesn't have a wall on either one side or both sides
+
+            int pulseCount = (encoder0.getPulses()-count) % 5600;
+            if (pulseCount > 5400 && pulseCount < 5800) {
+                blueLed.write(0);
+            } else {
+                blueLed.write(1);
+            }
+            sumError += currentError;
+            currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
+            derivError = currentError - previousError;
+            double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
+            if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
+                rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
+                leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
+            } else { // r is faster than L. speed up l, slow down r
+                rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
+                leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
+            }
+
+            if (leftSpeed > 0.30) leftSpeed = 0.30;
+            if (leftSpeed < 0) leftSpeed = 0;
+            if (rightSpeed > 0.30) rightSpeed = 0.30;
+            if (rightSpeed < 0) rightSpeed = 0;
+
+            right_motor.forward(rightSpeed);
+            left_motor.forward(leftSpeed);
+
+            previousError = currentError;
+
+            ir2 = IRP_2.getSamples( SAMPLE_NUM );
+            ir3 = IRP_3.getSamples( SAMPLE_NUM );
+
         }
 
-        if (leftSpeed > 0.30) leftSpeed = 0.30;
-        if (leftSpeed < 0) leftSpeed = 0;
-        if (rightSpeed > 0.30) rightSpeed = 0.30;
-        if (rightSpeed < 0) rightSpeed = 0;
-
-        right_motor.forward(rightSpeed);
-        left_motor.forward(leftSpeed);
-
-        previousError = currentError;
+        //backward();
+        wait_ms(40);
+        //brake();
 
-        ir2 = IRP_2.getSamples( SAMPLE_NUM );
-        ir3 = IRP_3.getSamples( SAMPLE_NUM );
-
+        left_motor.brake();
+        right_motor.brake();
     }
-
-    //backward();
-    wait_ms(40);
-    //brake();
-
-    left_motor.brake();
-    right_motor.brake();
 }
 
 void printDipFlag()
@@ -472,6 +591,8 @@
         ir3 = IRP_3.getSamples( SAMPLE_NUM );
     }
 
+
+
     left_motor.brake();
     right_motor.brake();
 }
@@ -536,23 +657,34 @@
     //left_motor.forward( 0.2 );
 
     while (1) {
+        
+        //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
+
         // oneCellEncoderAndIR(5);
-        // break;
-        moveForwardCellEncoder(1);
-        wait(0.5);
-        handleTurns();
-        wait(0.5);
-        moveForwardCellEncoder(1);
-        wait(0.5);
-        handleTurns();
-        break;
+        moveForwardUntilWallIr();
+        //moveForwardEncoder();
+        //serial.printf("ded \n");
+        //break;
+
+
+        //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
+
+
+        //break;
+        // moveForwardCellEncoder(1);
+        // wait(0.5);
+        // handleTurns();
+        // wait(0.5);
+        // moveForwardCellEncoder(1);
+        // wait(0.5);
+        // handleTurns();
+        //break;
         //pidOnEncoders();
        // moveForwardUntilWallIr();
         //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());
         //serial.printf("Pulse Count=> e0:%d, e1:%d      \r\n", encoder0.getPulses(),encoder1.getPulses());
         // double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
-        // serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n",
-        // IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError );
+        //serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError );
 
         //reading = Rec_4.read();
 //        serial.printf("reading: %f\n", reading);