square in terms of coordinates

control/control.ino

@@ -7,7 +7,7 @@ MeUltrasonicSensor mySensor(2);
 MeGyro myGyro;
 
 // Vars to change setup
-double Kp = -5.0;
+double Kp = -6.5;
 double target = 90;
 
 // robot characteristics
@@ -22,7 +22,7 @@ double error = 0.0; // in deg
 
 void setup() {
   // put your setup code here, to run once:
-  Serial.begin(115200);
+  Serial.begin(9600);
   _leftMotor.run(0);
   _rightMotor.run(0);
   myGyro.begin();
@@ -37,13 +37,16 @@ void loop() {
   // put your main code here, to run repeatedly:
     error = target - _thetaCurrent;
     //Serial.println(error);
-    if (error > 0.1){
+    while (error > 2 || error < -2){
+      Serial.println(error);
       adjustAngle(error, _distanceBetweenWheels, _wheelDiameter, Kp);
+      myGyro.update();
+      _thetaCurrent = myGyro.getAngleZ() - _thetaBaseline;
+      error = target - _thetaCurrent;
     }
     myGyro.update();
     _thetaCurrent = myGyro.getAngleZ() - _thetaBaseline;
     error = target - _thetaCurrent;
-
   
 }
 

gyro/gyro.ino

@@ -7,7 +7,8 @@ void setup() {
   // put your setup code here, to run once:
   Serial.begin(9600);
   myGyro.begin();
-  //myGyro.setpin();
+  delay(2000);
+  myGyro.update();
 }
 
 void loop() {
@@ -15,7 +16,5 @@ void loop() {
   myGyro.update();
   Serial.print("Z axis gyro ");
   Serial.println(myGyro.getAngleZ());
-  Serial.print("Y axis gyro ");
-  Serial.println(myGyro.getAngleY());
 
 }

square/square.ino

@@ -9,6 +9,7 @@ MeGyro myGyro;
 // Vars to change setup
 double Kp = -5;
 double target = 0;
+int changeTheta;
 int mSpeed = 150; //supposing those are rpms
 
 // robot characteristics
@@ -46,6 +47,46 @@ void setup() {
 void loop() {
   // put your main code here, to run repeatedly:
 
+  if (coordinates[0][0] < coordinates[1][0]){
+    target = -90;
+    if (coordinates[0][1] < coordinates[2][1]){
+        changeTheta = 90;
+      }
+      else{
+        changeTheta = -90;
+      }
+  }
+  if (coordinates[0][0] > coordinates[1][0]){
+    target = 90;
+    if (coordinates[0][1] < coordinates[2][1]){
+        changeTheta = -90;
+      }
+      else{
+        changeTheta = 90;
+      }
+  }
+
+  if (coordinates[0][0] == coordinates[1][0]){
+    if (coordinates[0][1] < coordinates[1][1]){
+      target = 0;
+      if (coordinates[0][0] < coordinates[2][0]){
+        changeTheta = -90;
+      }
+      else{
+        changeTheta = 90;
+      }
+    }
+    else{
+      target = 180;
+      if (coordinates[0][0] < coordinates[2][0]){
+        changeTheta = 90;
+      }
+      else{
+        changeTheta = -90;
+      }
+    }
+  }
+  
   while (loc < 4){
     //calculating distance and angle to next position
     xa = coordinates[loc][0];
@@ -79,10 +120,13 @@ void loop() {
       loc ++;
       itr = 1;
       Serial.println(target);
-      target = target + 90;
+      target = target + changeTheta;
       if (target > 180){
         target = 180 - target;
         }
+      if (target < -180){
+        target = target + 360;
+      }
     }
   }