Diff: main.cpp

Revision:

9:1d8e4da058cd

Parent:

8:a0760acdc59e

Child:

10:810d1849da9d

--- a/main.cpp	Fri May 05 01:21:33 2017 +0000
+++ b/main.cpp	Sat May 06 01:31:44 2017 +0000
@@ -9,12 +9,75 @@
 #include "QEI.h"
 
 // PID
-#define P_CONSTANT 0.0025
-#define I_CONSTANT 0.0000025
-#define D_CONSTANT 0.25
+#define P_CONSTANT 0
+#define I_CONSTANT 0
+#define D_CONSTANT 0
+
+#define IP_CONSTANT 6
+#define II_CONSTANT 0
+#define ID_CONSTANT 1
 
 #include "QEI.h"
 #define PULSES 880
+#define SAMPLE_NUM 100
+
+
+void moveForwardUntilWallIr() {    
+    double currentError = 0;
+    double previousError = 0;
+    double derivError = 0;
+    double sumError = 0;
+    
+    double HIGH_PWM_VOLTAGE = 0.2;
+    
+    double rightSpeed = 0.2;
+    double leftSpeed = 0.2;
+    
+    float ir2 = IRP_2.getSamples( SAMPLE_NUM );
+    float ir3 = IRP_3.getSamples( SAMPLE_NUM );
+    
+    while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) {       
+        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 );       
+        
+    }
+    //sensor1Turn = IR_Sensor1.read();
+    //sensor4Turn = IR_Sensor4.read();
+    
+    //backward();
+    wait_ms(40);
+    //brake();
+    
+    left_motor.brake();
+    right_motor.brake();
+    while( 1 )
+    {
+        
+    }
+        
+    
+}
 
 int main()
 {
@@ -35,8 +98,8 @@
     greenLed.write(0);
     blueLed.write(1);
     
-    //rightWheelForward(0.1);
-    //leftWheelForward(0.1);
+    //left_motor.forward(0.1);
+    //right_motor.forward(0.1);
 
     // PA_1 is A of right
     // PA_0 is B of right
@@ -46,11 +109,15 @@
     QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
 
     while (1) {
-        
+        moveForwardUntilWallIr();
         wait(0.1);
         //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());
-
+        //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 );
+        
         //reading = Rec_4.read();
 //        serial.printf("reading: %f\n", reading);
 //        redLed.write(0);