Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

Files at this revision

API Documentation at this revision

Comitter:
sahilmgandhi
Date:
Sat May 27 03:37:24 2017 +0000
Parent:
36:9c4cc9944b69
Child:
38:fe05f93009a2
Commit message:
Need to get centered more before turns (so after moving forward). Also fix when both walls are missing!

Changed in this revision

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
--- a/main.cpp	Sat May 27 02:40:10 2017 +0000
+++ b/main.cpp	Sat May 27 03:37:24 2017 +0000
@@ -38,9 +38,9 @@
 
 */
 
-#define IP_CONSTANT 1.93
-#define II_CONSTANT 0.0001
-#define ID_CONSTANT 0.175
+#define IP_CONSTANT 2.00
+#define II_CONSTANT 0.00001
+#define ID_CONSTANT 0.190
 
 void pidOnEncoders();
 
@@ -49,8 +49,8 @@
     int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum;
     int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum;
 
-    left_motor.forward(0.125);
-    right_motor.forward(0.095);
+    left_motor.forward(WHEEL_SPEED);
+    right_motor.forward(WHEEL_SPEED);
     wait_ms(1);
     while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1) {
         receiverTwoReading = IRP_2.getSamples(100);
@@ -86,8 +86,8 @@
     double kd = 0.00019;
     int prev = 0;
 
-    double speed0 = 0.11;
-    double speed1 = 0.13;
+    double speed0 = WHEEL_SPEED;
+    double speed1 = WHEEL_SPEED;
     right_motor.move(speed0);
     left_motor.move(speed1);
 
@@ -140,8 +140,8 @@
     double kd = 0.00019;
     int prev = 0;
 
-    double speed0 = 0.11;
-    double speed1 = 0.13;
+    double speed0 = WHEEL_SPEED;
+    double speed1 = WHEEL_SPEED;
     right_motor.move(speed0);
     left_motor.move(speed1);
 
@@ -350,18 +350,18 @@
     double derivError = 0;
     double sumError = 0;
 
-    double HIGH_PWM_VOLTAGE_R = 0.11;
-    double HIGH_PWM_VOLTAGE_L = 0.11;
+    double HIGH_PWM_VOLTAGE_R = WHEEL_SPEED;
+    double HIGH_PWM_VOLTAGE_L = WHEEL_SPEED;
 
-    double rightSpeed = 0.11;
-    double leftSpeed = 0.11;
+    double rightSpeed = WHEEL_SPEED;
+    double leftSpeed = WHEEL_SPEED;
 
     int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
     int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;
     serial.printf("%d, %d\n", desiredCount0, desiredCount1 );
 
-    left_motor.forward(0.11);
-    right_motor.forward(0.11);
+    left_motor.forward(WHEEL_SPEED);
+    right_motor.forward(WHEEL_SPEED);
 
     float ir2 = IRP_2.getSamples( SAMPLE_NUM );
     float ir3 = IRP_3.getSamples( SAMPLE_NUM );
@@ -375,7 +375,7 @@
         receiverOneReading = IRP_1.getSamples( SAMPLE_NUM );
         receiverFourReading = IRP_4.getSamples( SAMPLE_NUM );
         // serial.printf("IR2 = %f, IR2AVE = %f, IR3 = %f, IR3_AVE = %f\n", receiverTwoReading, IRP_2.sensorAvg, receiverThreeReading, IRP_3.sensorAvg);
-        if(  receiverOneReading > IRP_1.sensorAvg * 3.4 || receiverFourReading > IRP_4.sensorAvg * 3.4) {
+        if(  receiverOneReading > IRP_1.sensorAvg * 3.8 || receiverFourReading > IRP_4.sensorAvg * 3.8) {
             break;
         }
 
@@ -456,7 +456,7 @@
     // GO BACK A BIT BEFORE BRAKING??
     left_motor.backward(0.01);
     right_motor.backward(0.01);
-    wait_us(100);
+    wait_us(150);
     // DELETE THIS IF IT DOES NOT WORK!!
     
     left_motor.brake();
@@ -882,8 +882,8 @@
     //int currEncoder1Count = 0;
 
     //bool overrideFloodFill = false;
-    right_motor.forward( 0.11 );
-    left_motor.forward( 0.11 );
+    right_motor.forward( WHEEL_SPEED );
+    left_motor.forward( WHEEL_SPEED );
     //turn180();
     //wait_ms(1500);
     //int nextMovement = 0;
@@ -935,20 +935,13 @@
 
         //////////////////////// TESTING CODE GOES BELOW ///////////////////////////
         
-        turnLeft();
-        wait_ms(300);
-        turnLeft();
-        wait_ms(300);
-        turnLeft();
+        nCellEncoderAndIR(4);
         wait_ms(300);
-        turnLeft();
-        /*
+        turnRight();
+        nCellEncoderAndIR(1);
         wait_ms(300);
-        turn180();
-        wait_ms(300);
-        turn180();
-        wait_ms(300);*/
-        
+        turnRight();
+        nCellEncoderAndIR(4);        
         waitButton4();
         // serial.printf("Encoder 0 is : %d \t Encoder 1 is %d\n",encoder0.getPulses(), encoder1.getPulses() );
         // double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
--- a/main.h	Sat May 27 02:40:10 2017 +0000
+++ b/main.h	Sat May 27 03:37:24 2017 +0000
@@ -172,12 +172,12 @@
 //#define II_CONSTANT 0.06
 //#define ID_CONSTANT 7.55
 
-const int desiredCount180 = 3200;   // change accordingly to the terrain
+const int desiredCount180 = 3400;   // change accordingly to the terrain
 const int desiredCountR = 1600;
 const int desiredCountL = 1590;
  
 const int oneCellCount = 5400;
-const int oneCellCountMomentum = 4800;//4570 (.15) speed;//4800;      // one cell count is actually approximately 5400, but this value is considering momentum!
+const int oneCellCountMomentum = 4900;//4570 (.15) speed;//4800;      // one cell count is actually approximately 5400, but this value is considering momentum!
  
 double receiverOneReading = 0.0;
 double receiverTwoReading = 0.0;
@@ -198,7 +198,7 @@
     double speed0 = 0.11;
     double speed1 = -0.12;    // change back to 0.13 if turns stop working, testing something out!
  
-    double kp = 0.000085;
+    double kp = 0.000080;
  
     int counter = 0;
     int initial0 = encoder0.getPulses();
@@ -221,9 +221,7 @@
             
             error0 = desiredCount0 - count0; // is negative
             error1 = desiredCount1 - count1; // is positive
- 
-            //serial.printf("%f, %f\n", error0, error1 );
- 
+  
             right_motor.move(error1*kp);
             left_motor.move(error0*kp);
             counter = 0;
@@ -232,7 +230,6 @@
             right_motor.brake();
             left_motor.brake();
         }
- 
         if (counter > 60) {
             break;
         }
@@ -274,8 +271,6 @@
             error0 = desiredCount0 - count0; // is positive
             error1 = desiredCount1 - count1; // is negative
  
-            //serial.printf("%f, %f\n", error0, error1 );
- 
             right_motor.move(error1*kp);
             left_motor.move(error0*kp);
             counter = 0;
@@ -284,7 +279,6 @@
             right_motor.brake();
             left_motor.brake();
         }
- 
         if (counter > 60) {
             break;
         }
@@ -298,8 +292,10 @@
 
 inline void turn180()
 {
-    double speed0 = -0.15;
-    double speed1 = 0.16;
+    double speed0 = -0.10;
+    double speed1 = 0.11;
+    
+    double kp = 0.000055;
  
     int counter = 0;
     int initial0 = encoder0.getPulses();
@@ -314,25 +310,23 @@
     double error0 = count0 - desiredCount0;
     double error1 = count1 - desiredCount1;
  
- 
     while(1) {
  
-        if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
+        if(!(abs(error0) < 3) && !(abs(error1) < 3)) {
             count0 = encoder0.getPulses();
             count1 = encoder1.getPulses();
  
-            error0 = count0 - desiredCount0;
-            error1 = count1 - desiredCount1;
+            error0 = desiredCount0 - count0;
+            error1 = desiredCount1 - count1;
  
-            right_motor.move(speed0);
-            left_motor.move(speed1);
+            right_motor.move(error1*kp);
+            left_motor.move(error0*kp);
             counter = 0;
         } else {
             counter++;
             right_motor.brake();
             left_motor.brake();
         }
- 
         if (counter > 60) {
             break;
         }