Jim Dios / Mbed 2 deprecated MAU2

s

Dependencies:   mbed Motordriver

Files at this revision

API Documentation at this revision

Revision 0:23ce86997770, committed 2020-03-12

Comitter:
jdios
Date:
Thu Mar 12 20:29:39 2020 +0000
Commit message:
test

Changed in this revision

Motordriver.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motordriver.lib	Thu Mar 12 20:29:39 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/littlexc/code/Motordriver/#3110b9209d3c

--- /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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Mar 12 20:29:39 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file