Revision 7:e10dc3cb9212, committed 2017-11-29

Comitter:

Joshua_Cheung

Date:

Wed Nov 29 02:48:57 2017 +0000

Parent:

6:71829ae2ee07

Child:

8:6b2f7886768d

Commit message:

kinds of goes through maze, first turn

Changed in this revision

--- a/PID_control.cpp	Sat Nov 11 03:05:20 2017 +0000
+++ b/PID_control.cpp	Wed Nov 29 02:48:57 2017 +0000
@@ -1,16 +1,17 @@
+
 #include "mbed.h"
 #include "QEI.h"
 
 QEI encoder_Right(PB_3, PA_15, NC, 360, QEI::X4_ENCODING);
 QEI encoder_Left(PA_1, PC_4, NC, 360, QEI::X4_ENCODING);
-double Kp = 0.3;//.005;
+double Kp = 0.27;//.005;
 double Ki = 0.001;//0.0000001;
 double Kd = 0.001;
 PwmOut m_Right_F(PB_10);
 PwmOut m_Right_B(PC_7);
 PwmOut m_Left_F(PA_7);
 PwmOut m_Left_B(PB_6);
-double i_speed = 0.3;
+double i_speed = 0.15;
 double C_speed(0);
 int integrator = 0;
 int decayFactor = 1;
@@ -63,6 +64,7 @@
         C_speed = C_speed*-1;
 }
 
+
 void forward()
 {
     double f1_speed = i_speed + C_speed;
@@ -73,7 +75,8 @@
         pc.printf("-");
     if (C_speed > 0)
         pc.printf("+");
-        */
+    */
+
 
     if(f1_speed >= 0.7) {   //upper bound, can not go faster
         f1_speed = 0.7;
@@ -103,19 +106,48 @@
     }
 }
 
+void backUp()
+{
+    m_Left_F.write(0);
+    m_Right_F.write(0);
+    m_Left_B.write(i_speed);
+    m_Right_B.write(i_speed);
+    wait(.15);
+}
+
 void turnRight()
 {
-
+    m_Left_B.write(0);
+    m_Right_F.write(0);
+    m_Left_F.write(i_speed);
+    m_Right_B.write(i_speed);
+    wait(.2);
 }
 
 void turnLeft()
 {
-
+    m_Left_F.write(0);
+    m_Right_B.write(0);
+    m_Right_F.write(i_speed);
+    m_Left_B.write(i_speed);
+    wait(.2);
 }
 
 void turnAround()
 {
+    m_Left_B.write(0);
+    m_Right_F.write(0);
+    m_Left_F.write(i_speed);
+    m_Right_B.write(i_speed);
+    wait(.4);
+}
 
+void stop()
+{
+    m_Right_F.write(0);
+    m_Right_B.write(0);
+    m_Left_F.write(0);
+    m_Left_B.write(0);    
 }
 
 void debugEncoder()
@@ -135,7 +167,76 @@
     }
 }
 
-int main()      //only runs once
+AnalogIn RS_IRR(PA_0);
+AnalogIn RF_IRR(PA_4); //Right Front
+AnalogIn LF_IRR(PC_1); //Left Front
+AnalogIn LS_IRR(PC_0); //Left Side
+ 
+DigitalOut RS_IRE(PC_10);
+DigitalOut RF_IRE(PC_11); //Right Front
+DigitalOut LF_IRE(PB_0); //Left Front
+DigitalOut LS_IRE(PB_7); //Left Side 
+
+ 
+
+int main()
+{
+    float threshold = 0.001;
+    float turnThreshold = 0.001;
+    printf("\nAnalogIn example\n");
+    LF_IRE.write(1);
+    RF_IRE.write(1);  
+    while (1){ 
+    while (LF_IRR.read() < threshold && RF_IRR.read() < threshold){
+        forward();
+        float value1 = LF_IRR.read();
+        float value2 = RF_IRR.read(); 
+        printf("LF Led: %f\n", value1);
+        wait(0.5);
+        printf("RF Led: %f\n", value2);
+    }
+    
+    backUp();
+    
+    if (LS_IRR.read() > turnThreshold)
+        if (RS_IRR.read() < turnThreshold)
+            turnRight();
+        else
+            turnAround();
+    else if (RS_IRR.read() > turnThreshold)
+        if (LS_IRR.read() < turnThreshold)
+            turnRight();
+        else
+            turnAround();
+    else
+        turnAround();
+}
+    stop();
+}
+    /*while(RF_IRR.read() * 100000 < 175 && LF_IRR.read() * 100000 < 175) {
+        
+        /*meas = LS_IRR.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
+        meas = meas * 100000; // Change the value to be in the 0 to 3300 range
+        printf("measure = %.0f mV\n", meas);
+        */
+        /*if (meas > 2000) { // If the value is greater than 2V then switch the LED on
+          LS_IRE = 1;
+        }
+        else {
+          LS_IRE = 0;
+        }
+        */
+        /*
+        forward();
+        //wait(0.2); // 200 ms
+    }
+    stop();
+    */
+    
+
+
+
+/*int main()      //only runs once
 {
     systicker.attach_us(&systick, 1000);    //enable interrupt
     while (1) {
@@ -144,3 +245,4 @@
     //
     //debugEncoder();
 }
+*/

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RemoteIR.lib	Wed Nov 29 02:48:57 2017 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/shintamainjp/code/RemoteIR/#268cc2ab63bd

--- a/main.cpp	Sat Nov 11 03:05:20 2017 +0000
+++ b/main.cpp	Wed Nov 29 02:48:57 2017 +0000
@@ -1,5 +1,7 @@
 /*
 #include "mbed.h"
+Serial pc (PA_2, PA_3);
+/*
 #include "QEI.h"
 Serial pc (PA_2, PA_3); //serial comm enabled on pins pa_2 and pa_3
 QEI encoder_Right(PB_3, PA_15, NC, 360, QEI::X4_ENCODING);
@@ -10,6 +12,18 @@
 PwmOut m_Left_B(PB_6); 
 */
 
+/*//IRR = IR Reciver
+AnalogIn RS_IRR(PA_0); //Right Side 
+AnalogIn RF_IRR(PA_4); //Right Front
+AnalogIn LF_IRR(PC_1); //Left Front
+AnalogIn LS_IRR(PC_0); //Left Side
+
+//IRE = IR Emitter
+DigitalOut RS_IRE(PC_10); //Right Side
+DigitalOut RF_IRE(PC_11); //Right Front
+DigitalOut LF_IRE(PB_0); //Left Front
+DigitalOut LS_IRE(PB_7); //Left Side 
+
 //QEI functions
 /*
 void    reset (void)
@@ -21,20 +35,17 @@
 int     getRevolutions (void)
     Read the number of revolutions recorded by the encoder on the index channel.
 */
+
 /*
 int main()
 {
-    m_Right_F.write(0.5);
-    //m_Left_F.write(.25);
-    //m_Left_B.write(.25); 
-    while(1) {
-        wait(0.1);
-        
-        pc.printf("Pulses is: %i\n", encoder_Right.getPulses());
-        //pc.printf("Pulses is: %i\n", encoder_Left.getPulses());
+    while(1)
+    {
+        LS_IRE.write(1);
+        float value = LS_IRR.read();
+        printf("IR Led: %f", value);
+        wait(100);
+        LS_IRE.write(0);    
     }
-    
-    
-
 }
 */
\ No newline at end of file

--- a/mbed.bld	Sat Nov 11 03:05:20 2017 +0000
+++ b/mbed.bld	Wed Nov 29 02:48:57 2017 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/mbed_official/code/mbed/builds/fb8e0ae1cceb
\ No newline at end of file
+https://os.mbed.com/users/mbed_official/code/mbed/builds/e7ca05fa8600
\ No newline at end of file