Diff: Mapping.cpp

Revision:

1:7758ee895593

Child:

2:d439dbbc522f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Mapping.cpp	Wed May 28 11:19:33 2014 +0000
@@ -0,0 +1,302 @@
+#include "mbed.h"
+#include "m3pi_ng.h"
+ 
+m3pi MarkII;
+ 
+ 
+int main() {
+    
+    return 0;
+    
+}
+
+void mapping(m3pi thinggy, float speed, int startpoint, int endpoint, bool intersection){
+    int count = 0;
+    
+    switch(startpoint){
+        case 1:
+            if(endpoint == 2){
+                while(count < 2){
+                    if(intersection == true){
+                        count++;
+                        intersection = false;
+                        if(count == 1){
+                            thinggy.left(speed);
+                        }
+                        if(count == 2){
+                            thinggy.left(speed);
+                        }
+                    }
+                }
+                count = 0;
+            }
+            else if(endpoint == 3){
+                while(count < 3){
+                    if(intersection == true){
+                        count++;
+                        intersection = false;
+                        if(count == 1){
+                            thinggy.left(speed);
+                        }
+                        if(count == 2){
+                            thinggy.forward(speed);
+                        }
+                        if(count == 2){
+                            thinggy.forward(speed);
+                        }
+                    }
+                }
+                count = 0;
+                }
+            else if(endpoint == 4){
+                while(count < 4){
+                    if(intersection == true){
+                        count++;
+                        intersection = false;
+                        if(count == 1){
+                            thinggy.left(speed);
+                        }
+                        if(count == 2){
+                            thinggy.forward(speed);
+                        }
+                        if(count == 3){
+                            thinggy.right(speed);
+                        }
+                        if(count == 4){
+                            thinggy.left(speed);
+                        }
+                    }
+                }
+                count = 0;
+                }
+            else if(endpoint == 5){
+                while(count < 4){
+                    if(intersection == true){
+                        count++;
+                        intersection = false;
+                        if(count == 1){
+                            thinggy.right(speed);
+                        }
+                        if(count == 2){
+                            thinggy.forward(speed);
+                        }
+                        if(count == 3){
+                            thinggy.right(speed);
+                        }
+                        if(count == 4){
+                            thinggy.right(speed);
+                        }
+                    }
+                }
+                count = 0;
+                }
+            else if(endpoint == 6){
+                while(count < 2){
+                    if(intersection == true){
+                        count++;
+                        intersection = false;
+                        if(count == 1){
+                            thinggy.left(speed);
+                        }
+                        if(count == 2){
+                            thinggy.forward(speed);
+                        }
+                    }
+                }
+                count = 0;
+                }
+        break;
+        
+        case 2:
+        if(endpoint == 2){
+                while(count < 2){
+                    if(intersection == true){
+                        count++;
+                        intersection = false;
+                        if(count == 1){
+                            thinggy.right(speed);
+                        }
+                        if(count == 2){
+                            thinggy.right(speed);
+                        }
+                    }
+                }
+                count = 0;
+            }
+            else if(endpoint == 3){
+                while(count < 2){
+                    if(intersection == true){
+                        count++;
+                        intersection = false;
+                        if(count == 1){
+                            thinggy.left(speed);
+                        }
+                        if(count == 2){
+                            thinggy.right(speed);
+                        }
+                    }
+                }
+                count = 0;
+                }
+            else if(endpoint == 4){
+                while(count < 2){
+                    if(intersection == true){
+                        count++;
+                        intersection = false;
+                        if(count == 1){
+                            thinggy.right(speed);
+                        }
+                        if(count == 2){
+                            thinggy.left(speed);
+                        }
+                    }
+                }
+                count = 0;
+                }
+            else if(endpoint == 5){
+                while(count < 3){
+                    if(intersection == true){
+                        count++;
+                        intersection = false;
+                        if(count == 1){
+                            thinggy.forward(speed);
+                        }
+                        if(count == 2){
+                            thinggy.left(speed);
+                        }
+                        if(count == 3){
+                            thinggy.right(speed);
+                        }
+                    }
+                }
+                count = 0;
+                }
+            else if(endpoint == 6){
+                while(count < 3){
+                    if(intersection == true){
+                        count++;
+                        intersection = false;
+                        if(count == 1){
+                            thinggy.right(speed);
+                        }
+                        if(count == 2){
+                            thinggy.forward(speed);
+                        }
+                        if(count == 2){
+                            thinggy.right(speed);
+                        }
+                    }
+                }
+                count = 0;
+                }
+        
+        
+        break;
+        
+        case 3:
+        if(endpoint == 2){
+                while(count < 2){
+                    if(intersection == true){
+                        count++;
+                        intersection = false;
+                        if(count == 1){
+                            thinggy.right(speed);
+                        }
+                        if(count == 2){
+                            thinggy.right(speed);
+                        }
+                    }
+                }
+                count = 0;
+            }
+            else if(endpoint == 3){
+                while(count < 2){
+                    if(intersection == true){
+                        count++;
+                        intersection = false;
+                        if(count == 1){
+                            thinggy.left(speed);
+                        }
+                        if(count == 2){
+                            thinggy.right(speed);
+                        }
+                    }
+                }
+                count = 0;
+                }
+            else if(endpoint == 4){
+                while(count < 2){
+                    if(intersection == true){
+                        count++;
+                        intersection = false;
+                        if(count == 1){
+                            thinggy.right(speed);
+                        }
+                        if(count == 2){
+                            thinggy.left(speed);
+                        }
+                    }
+                }
+                count = 0;
+                }
+            else if(endpoint == 5){
+                while(count < 3){
+                    if(intersection == true){
+                        count++;
+                        intersection = false;
+                        if(count == 1){
+                            thinggy.forward(speed);
+                        }
+                        if(count == 2){
+                            thinggy.left(speed);
+                        }
+                        if(count == 3){
+                            thinggy.right(speed);
+                        }
+                    }
+                }
+                count = 0;
+                }
+            else if(endpoint == 6){
+                while(count < 4){
+                    if(intersection == true){
+                        count++;
+                        intersection = false;
+                        if(count == 1){
+                            thinggy.forward(speed);
+                        }
+                        if(count == 2){
+                            thinggy.forward(speed);
+                        }
+                        if(count == 3){
+                            thinggy.forward(speed);
+                        }
+                        if(count == 4){
+                            thinggy.right(speed);
+                        }
+                    }
+                }
+                count = 0;
+                }
+        
+        
+        break;
+        
+        case 4:
+        
+        
+        break;
+        
+        case 5:
+        
+        
+        break;
+        
+        case 6:
+        
+        
+        break;
+        default:
+    }
+    
+}
\ No newline at end of file