Diff: main.cpp

Revision:

37:3dcc95e9321c

Parent:

36:9c4cc9944b69

Child:

38:fe05f93009a2

--- 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) ) ;