MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

Files at this revision

API Documentation at this revision

Revision 17:f713758f6238, committed 2017-05-14

Comitter:
sahilmgandhi
Date:
Sun May 14 23:07:23 2017 +0000
Parent:
16:d9252437bd92
Child:
18:6a4db94011d3
Commit message:
Added mbed-dev and still working on dynamically deciding to turn;

Changed in this revision

deprecatedMethods.cpp Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
main.h Show annotated file Show diff for this revision Revisions of this file
mbed-dev.lib Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show diff for this revision Revisions of this file 
--- a/deprecatedMethods.cpp	Sun May 14 20:58:55 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,202 +0,0 @@
-//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();
-//}
\ No newline at end of file

--- a/main.cpp	Sun May 14 20:58:55 2017 +0000
+++ b/main.cpp	Sun May 14 23:07:23 2017 +0000
@@ -26,6 +26,11 @@
 const int oneCellCount = 5400;
 const int oneCellCountMomentum = 4600;      // one cell count is actually approximately 5400, but this value is considering momentum!
 
+float receiverOneReading = 0.0;
+float receiverTwoReading = 0.0;
+float receiverThreeReading = 0.0;
+float receiverFourReading = 0.0;
+
 void pidOnEncoders();
 
 void turnLeft()
@@ -72,6 +77,7 @@
 
     right_motor.brake();
     left_motor.brake();
+    turnFlag = 0;           // zeroing out the flags!
 }
 
 void turnRight()
@@ -118,6 +124,7 @@
 
     right_motor.brake();
     left_motor.brake();
+    turnFlag = 0;
 }
 
 void turnLeft180()
@@ -211,14 +218,20 @@
     left_motor.brake();
 }
 
-void moveForwardOneCellEncoder(){
-    int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum;
-    int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum;
+void moveForwardCellEncoder(double cellNum){
+    int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum;
+    int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum;
 
     left_motor.forward(0.12);
     right_motor.forward(0.10);
     wait_ms(2);
     while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
+        receiverTwoReading = IRP_2.getSamples(20);
+        receiverThreeReading = IRP_3.getSamples(20);
+        if (receiverTwoReading <= IRP_2.sensorAvg/2.5)
+            turnFlag |= LEFT_FLAG;
+        else if (receiverThreeReading <= IRP_3.sensorAvg/2.5)
+            turnFlag |= RIGHT_FLAG;
         // serial.printf("Encoder0 count is: %d, Encoder1 count is: %d, desiredEncoder0 = %d, desiredEncoder1 = %d\n", encoder0.getPulses(), encoder1.getPulses(), desiredCount0, desiredCount1);
         pidOnEncoders();
     }
@@ -227,6 +240,21 @@
     right_motor.brake();
 }
 
+void handleTurns(){
+    if (turnFlag == 0x1){
+        moveForwardCellEncoder(0.5);
+        turnLeft();
+    }
+    else if (turnFlag == 0x2){
+        moveForwardCellEncoder(0.5);
+        turnRight();
+    }
+    else if (turnFlag == 0x3){
+        moveForwardCellEncoder(0.5);
+        turnLeft();
+    }
+}
+
 void moveForwardUntilWallIr()
 {
     double currentError = 0;
@@ -251,7 +279,6 @@
             blueLed.write(1);
         }
 
-
         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;
@@ -379,6 +406,10 @@
     }
 }
 
+void oneCellEncoderAndIR(){
+
+}
+
 int main()
 {
     //Set highest bandwidth.
@@ -439,21 +470,26 @@
     //left_motor.forward( 0.2 );
 
     while (1) {
-        moveForwardOneCellEncoder();
+        moveForwardCellEncoder(1);
+        wait(0.3);
+        handleTurns();
+        wait_ms(5);
+        moveForwardCellEncoder(1);
+        wait(0.3);
+        handleTurns();
+        break;
+        // moveForwardOneCellEncoder();
         //pidOnEncoders();
        //moveForwardUntilWallIr();
     //        wait(2);
     //        turnRight();
     //        wait(1);
     //        moveForwardUntilWallIr();
-
-        break;
         //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 );
+        // 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 );
 
         //reading = Rec_4.read();
 //        serial.printf("reading: %f\n", reading);

--- a/main.h	Sun May 14 20:58:55 2017 +0000
+++ b/main.h	Sun May 14 23:07:23 2017 +0000
@@ -78,6 +78,10 @@
 #define BUTTON3_FLAG 0x4
 #define BUTTON4_FLAG 0x8
 
+int turnFlag = 0;
+#define LEFT_FLAG 0x1
+#define RIGHT_FLAG 0x2
+
 QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
 QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-dev.lib	Sun May 14 23:07:23 2017 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/mbed_official/code/mbed-dev/#289d4deac6e4

--- a/mbed.bld	Sun May 14 20:58:55 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://mbed.org/users/mbed_official/code/mbed/builds/97feb9bacc10
\ No newline at end of file