Diff: main.cpp

Revision:

0:23ce86997770

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Mar 12 20:29:39 2020 +0000
@@ -0,0 +1,138 @@
+#include "mbed.h"
+#include "motordriver.h"
+#include "math.h"
+ 
+// Middle = M, Left = L, Right = R
+ 
+Motor RA(p23, p30, p24, 1);
+Motor LA(p22, p29, p24, 1);
+Motor MA(p21, p26, p24, 1);
+ 
+Motor MB(p21, p24, p26, 1);
+Motor RB(p23, p24, p30, 1);
+Motor LB(p22, p24, p29, 1);
+ 
+AnalogIn SML(p17); // sensor middle and left above right whell
+AnalogIn SMR(p19); // sensor middle and right above left whell
+AnalogIn SRL(p20); // sensor right and left above middle whell
+ 
+void stop()
+{
+    MA.coast();
+    LA.coast();
+    RA.coast();
+    MB.coast();
+    LB.coast();
+    RB.coast();
+}
+ 
+float getDistance(float a, float b, float c, float x)
+{
+    return (a - b*x + c*pow(x,2));
+}
+ 
+int main()
+{
+    while(1){
+ 
+        // move back SML - Right
+        // y = 99.77605 - 382.7846*x + 398.975*x^2
+        float distanceSML = getDistance(99.77605, 382.7846, 398.975 , SML.read()); // TODO: put parameter for this sensor for a and b
+        // backSML(0.3, 1000);
+
+        // move back SLR - Middle
+        // y = 99.71885 - 382.2867*x + 398.2079*x^2
+        float distanceSLR = getDistance(99.71885, 384.0377, 400.3844, SRL.read()); // TODO: put parameter for this sensor for a and b
+        // backSLR(speedSLR, durationSLR);
+
+        // move back SMR - Left
+        // y = 99.97613 - 384.0377*x + 400.3844*x^2
+        float distanceSMR = getDistance(99.97613, 384.0377, 400.3844, SMR.read()); // TODO: put parameter for this sensor for a and b
+        // backSMR(speedSMR, durationSMR);
+       
+        if(distanceSMR <= 9.5 && distanceSLR <= 9.5 && distanceSML > 9.5){
+            MB.speed(0.7);
+            LA.speed(0.7);
+            RA.speed(0.0);
+        } else if(distanceSMR > 9.5 && distanceSLR > 9.5 && distanceSML <= 9.5){
+            MA.speed(0.7);
+            LB.speed(0.7);
+            RA.speed(0.0);
+        }
+        
+        if(distanceSML <= 9.5 && distanceSLR <= 9.5 && distanceSMR > 9.5 ){  
+            RB.speed(0.7);
+            MA.speed(0.7);
+            LA.speed(0.0);
+        } else if(distanceSML > 9.5 && distanceSLR > 9.5 && distanceSMR <= 9.5 ){  
+            RA.speed(0.7);
+            MB.speed(0.7);
+            LA.speed(0.0);
+        }
+
+        if(distanceSML <= 9.5 && distanceSMR <= 9.5 && distanceSLR > 9.5){
+            LB.speed(0.7);
+            RA.speed(0.7);
+            MB.speed(0.0);     
+        } else if(distanceSML > 9.5 && distanceSMR > 9.5 && distanceSLR <= 9.5){
+            LA.speed(0.7);
+            RB.speed(0.7);
+            MB.speed(0.0);
+        }
+        
+        // spin 
+
+        while(distanceSML > 11 && distanceSLR > 11 && distanceSMR > 11) {
+            RA.speed(0.3);
+            MA.speed(0.3);
+            LA.speed(0.3);
+            // move back SML - Right
+            // y = 99.77605 - 382.7846*x + 398.975*x^2
+            float distanceSML = getDistance(99.77605, 382.7846, 398.975 , SML.read()); // TODO: put parameter for this sensor for a and b
+            // backSML(0.3, 1000);
+
+            // move back SLR - Middle
+            // y = 99.71885 - 382.2867*x + 398.2079*x^2
+            float distanceSLR = getDistance(99.71885, 384.0377, 400.3844, SRL.read()); // TODO: put parameter for this sensor for a and b
+            // backSLR(speedSLR, durationSLR);
+
+            // move back SMR - Left
+            // y = 99.97613 - 384.0377*x + 400.3844*x^2
+            float distanceSMR = getDistance(99.97613, 384.0377, 400.3844, SMR.read()); // TODO: put parameter for this sensor for a and b
+            // backSMR(speedSMR, durationSMR);
+        
+            if(distanceSMR <= 9.5 && distanceSLR <= 9.5 && distanceSML > 9.5){
+                MB.speed(0.7);
+                LA.speed(0.7);
+                RA.speed(0.0);
+            } else if(distanceSMR > 9.5 && distanceSLR > 9.5 && distanceSML <= 9.5){
+                MA.speed(0.7);
+                LB.speed(0.7);
+                RA.speed(0.0);
+            }
+            
+            if(distanceSML <= 9.5 && distanceSLR <= 9.5 && distanceSMR > 9.5 ){  
+                RB.speed(0.7);
+                MA.speed(0.7);
+                LA.speed(0.0);
+            } else if(distanceSML > 9.5 && distanceSLR > 9.5 && distanceSMR <= 9.5 ){  
+                RA.speed(0.7);
+                MB.speed(0.7);
+                LA.speed(0.0);
+            }
+
+            if(distanceSML <= 9.5 && distanceSMR <= 9.5 && distanceSLR > 9.5){
+                LB.speed(0.7);
+                RA.speed(0.7);
+                MB.speed(0.0);     
+            } else if(distanceSML > 9.5 && distanceSMR > 9.5 && distanceSLR <= 9.5){
+                LA.speed(0.7);
+                RB.speed(0.7);
+                MB.speed(0.0);
+            }
+        }
+
+
+        stop();
+    }    
+}
\ No newline at end of file