Revision 9:1d8e4da058cd, committed 2017-05-06

Comitter:

kyleliangus

Date:

Sat May 06 01:31:44 2017 +0000

Parent:

8:a0760acdc59e

Child:

10:810d1849da9d

Commit message:

IR PID is now implemented

Changed in this revision

--- a/irpair.cpp	Fri May 05 01:21:33 2017 +0000
+++ b/irpair.cpp	Sat May 06 01:31:44 2017 +0000
@@ -3,9 +3,12 @@
 
 void IRPair::calibrateSensor() {
 
+    ir.write( 1 );
+
     for (int i = 0; i < samplesToTake; ++i) 
         sensorAvg += recv.read();
         
+    ir.write( 0 );
     sensorAvg /= samplesToTake;
 }
 

--- a/irpair.h	Fri May 05 01:21:33 2017 +0000
+++ b/irpair.h	Sat May 06 01:31:44 2017 +0000
@@ -15,13 +15,15 @@
         
         float getSamples( int i );
         
+        float sensorAvg;
+        
     private:
         void calibrateSensor();
         
         // internal values
         DigitalOut ir;
         AnalogIn recv;
-        float sensorAvg;
+        
 };
 
 

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

--- a/main.h	Fri May 05 01:21:33 2017 +0000
+++ b/main.h	Sat May 06 01:31:44 2017 +0000
@@ -3,6 +3,7 @@
 
 #include "mbed.h"
 #include "ITG3200.h"
+#include "motor.h"
 
 // Motors
 /*
@@ -20,18 +21,26 @@
 DigitalOut blueLed(PC_1);
 DigitalOut greenLed(PC_2);
 
-// IRs
+// IRPairs
+IRPair IRP_4( PB_1, PC_5 );
+IRPair IRP_3( PB_13, PC_4 );
+IRPair IRP_2( PB_0, PA_6 );
+IRPair IRP_1( PB_14, PA_7 );
+
+Motor left_motor( PB_7, PB_8, PB_4 );
+Motor right_motor( PA_10, PA_11, PB_5 );
+
+/*
 DigitalOut IR_1(PB_1);
 DigitalOut IR_2(PB_13);
 DigitalOut IR_3(PB_0);
 DigitalOut IR_4(PB_14);
-
 // Receivers
 AnalogIn Rec_1(PC_5);
 AnalogIn Rec_2(PC_4);
 AnalogIn Rec_3(PA_6);
 AnalogIn Rec_4(PA_7);
-
+*/
 
 // Doing DEBUGGING
 #define DEBUGGING 1