added control, code has bugs

control/control.ino

@@ -0,0 +1,183 @@
+
+#include <MeMCore.h>
+#include <Wire.h>
+
+/*
+ * initialize array
+ * iterate through array
+ * 
+ * general function setting angle and distance based on target and current pos as inputs
+ * 
+ * if not travelling
+ *  iterate to next point in array
+ *  run function
+ *  turn according to output
+ *  set travelling true
+ *  set travel timer based on distance
+ *  set motors active
+ * if travelling
+ *  check timer
+ *  if timer done
+ *    set travelling not true
+ *    kill motors
+ *
+ */
+MeDCMotor motorL(M1); //positive go backward
+
+MeDCMotor motorR(M2); //positive go forward
+
+MeGyro gyro;
+
+
+uint8_t motorSpeed = 100;
+bool travelling = false;
+unsigned long travelTimer = 0;
+
+unsigned long travelTimerDuration = 0;
+
+float absoluteAngle = 0;
+float travelDistance = 0;
+
+float angleThreshold = 1;
+
+float robotSpeedInverse = 8000; //in ms/m
+
+//for point a    x = [0][ax], y = [1][ay]
+int positionArray[2][4] = {
+  {0, 1, 1, 0},
+  {0, 0, 1, 1}
+};
+
+int posIter = 0;
+int maxPosIter = 3;
+
+int xCurrent = 0;
+int yCurrent = 0;
+
+int xTarget = 0;
+int yTarget = 0;
+
+
+float distCalc(int x1, int y1, int x2, int y2){
+  float distance;
+  distance = sqrt(sq(x2-x1)+sq(y2-y1));
+  return distance;
+}
+
+float angleCalc(int x1, int y1, int x2, int y2){
+  float angle;
+  
+  if (x2-x1>0 && y2-y1>0){
+   atan2(y2-y1,x2-x1);
+  }
+  if (x2-x1<0 && y2-y1>0){
+   atan2(y2-y1,x2-x1)+PI/2;
+  }
+  if (x2-x1<0 && y2-y1<0){
+   atan2(y2-y1,x2-x1)+PI;
+  }   
+  if (x2-x1>0 && y2-y1<0){
+   atan2(y2-y1,x2-x1)+(3*PI)/2;
+  } 
+  return angle;
+}
+
+
+void setup()
+{
+  Serial.begin(9600);
+  gyro.begin();
+}
+
+void loop()
+{
+  if (travelling){
+    //timercheck
+    if(millis()- travelTimer >= travelTimerDuration){
+      //reset timer kill motors
+      travelling = false;
+      motorL.run(0);
+      motorR.run(0);      
+    }else{
+      //keep on travelling
+    }
+  }else{
+    //arrived at destination, iterate to new target and go there
+
+    //iterate tru array
+    
+    //obtain absoluteAngle and travelDistance from function with currentPos and targetPos as inputs
+
+    xCurrent = positionArray[0][posIter];
+    yCurrent = positionArray[1][posIter];
+
+    if(posIter == maxPosIter){
+      posIter = 0;
+      
+    }else{
+      posIter += 1;
+    }
+
+    xTarget = positionArray[0][posIter];
+    yTarget = positionArray[1][posIter];
+
+
+
+    absoluteAngle = angleCalc(xCurrent, yCurrent, xTarget, yTarget);
+    travelDistance = distCalc(xCurrent, yCurrent, xTarget, yTarget);
+ Serial.println(absoluteAngle);
+
+    //convert to degrees
+    absoluteAngle = (absoluteAngle*71)/4068;
+     Serial.println(absoluteAngle);
+
+    //turn the robot correctly
+    gyro.update();
+    while(abs(absoluteAngle-gyro.getAngleZ()) > angleThreshold ){
+      gyro.update();
+      motorL.run(motorSpeed);
+      motorR.run(motorSpeed);
+      
+    }
+    motorL.stop();
+    motorR.stop();
+
+
+    //set the travel state
+    travelling = true;
+    
+    travelTimerDuration = travelDistance*robotSpeedInverse;
+
+    Serial.println(absoluteAngle);
+    Serial.println(travelDistance);
+    Serial.println(travelTimerDuration);
+    
+    motorL.run(-motorSpeed);
+    motorR.run(motorSpeed); 
+    travelTimer = millis();   
+  }
+  
+  /*
+  Serial.print("Distance : ");
+  Serial.print(ultraSensor.distanceCm() );
+  Serial.println(" cm");
+  //delay(100);//docs say at least that timer
+  */
+  /*motorL.run(motorSpeed); // value: between -255 and 255. 
+  /motorR.run(motorSpeed); // value: between -255 and 255. 
+
+  delay(2000);
+  motorL.stop();
+  motorR.stop();
+
+  delay(100);
+  motorL.run(-motorSpeed);
+  motorR.run(-motorSpeed);
+
+  delay(2000);
+  motorL.stop();
+  motorR.stop();
+
+  delay(2000);
+  */
+}