square

square/square.ino

@@ -7,9 +7,9 @@ MeUltrasonicSensor mySensor(2);
 MeGyro myGyro;
 
 // Vars to change setup
-double Kp = -5.0;
-double target = 90;
-int mSpeed = 120; //supposing those are rpms
+double Kp = -5;
+double target = 0;
+int mSpeed = 150; //supposing those are rpms
 
 // robot characteristics
 double _wheelDiameter = 6.0; //cm
@@ -21,7 +21,7 @@ double _thetaCurrent = 0.0;
 double _thetaBaseline = 0.0;
 double error = 0.0; // in deg
 
-int coordinates[4][2] = {{0, 0}, {0, 1}, {1, 1}, {1, 0}}; //1 unit is 20 cm
+int coordinates[4][2] = {{0, 0}, {0, 2}, {2, 2}, {2, 0}}; //1 unit is 20 cm
 int itr = 1; //to sequence the movement between coordinates
 int loc = 0; //to scan through the coordinates
 
@@ -59,32 +59,30 @@ void loop() {
       yb = coordinates[0][1];
     }
     double dist = sqrt(sq(xb-xa) + sq(yb-ya));
-    
-    itr = 1;
-    while (itr < 5){
       //checking if the Z axis of the robot is aligned witht the theroretical axis
-      error = target - _thetaCurrent;
-      while (error > 2 || error < -2){
-        adjustAngle(error, _distanceBetweenWheels, _wheelDiameter, Kp);
-        myGyro.update();
-        _thetaCurrent = myGyro.getAngleZ() - _thetaBaseline;
-        error = target - _thetaCurrent;
-      }
+    error = target - _thetaCurrent;
+    //Serial.println(error);
+    while (error > 4 || error < -4){
+      adjustAngle(error, _distanceBetweenWheels, _wheelDiameter, Kp);
       myGyro.update();
       _thetaCurrent = myGyro.getAngleZ() - _thetaBaseline;
       error = target - _thetaCurrent;
-
-      getMoving(mSpeed, 1, -1, 500*dist);
-
-      itr ++;
     }
-    loc ++;
-    
+    myGyro.update();
+    _thetaCurrent = myGyro.getAngleZ() - _thetaBaseline;
+    error = target - _thetaCurrent;
+
+    getMoving(mSpeed, 1, -1, 200*dist);
+
+    itr ++;
+    if (itr == 5){
+      loc ++;
+      itr = 1;
       Serial.println(target);
       target = target + 90;
       if (target > 180){
         target = 180 - target;
-      }
+        }
     }
   }