square files

square/square.ino

@@ -0,0 +1,121 @@
+#include <MeMCore.h>
+#include <Wire.h>
+
+MeDCMotor _rightMotor(10);
+MeDCMotor _leftMotor(9);
+MeUltrasonicSensor mySensor(2);
+MeGyro myGyro;
+
+// Vars to change setup
+double Kp = -5.0;
+double target = 90;
+int mSpeed = 120; //supposing those are rpms
+
+// robot characteristics
+double _wheelDiameter = 6.0; //cm
+double _distanceBetweenWheels = 11.5; //cm
+int _basicMotorSpeed = 100;
+
+// position, baseline...
+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 itr = 1; //to sequence the movement between coordinates
+int loc = 0; //to scan through the coordinates
+
+int xa;
+int ya;
+int xb;
+int yb;
+
+void setup() {
+  // put your setup code here, to run once:
+  Serial.begin(9600);
+  _leftMotor.run(0);
+  _rightMotor.run(0);
+  myGyro.begin();
+  delay(2000);
+  myGyro.update();
+  
+  _thetaBaseline = myGyro.getAngleZ();
+  
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+
+  while (loc < 4){
+    //calculating distance and angle to next position
+    xa = coordinates[loc][0];
+    ya = coordinates[loc][1];
+    if (loc < 3){
+      xb = coordinates[loc+1][0];
+      yb = coordinates[loc+1][1];
+    }
+    else{
+      xb = coordinates[0][0];
+      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;
+      }
+      myGyro.update();
+      _thetaCurrent = myGyro.getAngleZ() - _thetaBaseline;
+      error = target - _thetaCurrent;
+
+      getMoving(mSpeed, 1, -1, 500*dist);
+
+      itr ++;
+    }
+    loc ++;
+    
+      Serial.println(target);
+      target = target + 90;
+      if (target > 180){
+        target = 180 - target;
+      }
+    }
+  }
+  
+}
+
+
+void getMoving(double speedMove, int dirRight, int dirLeft, int timeMove){
+  //if direction 1 -1 forward
+  _leftMotor.run(dirLeft*speedMove);
+  _rightMotor.run(dirRight*speedMove);
+  delay(timeMove);
+  _leftMotor.run(0);
+  _rightMotor.run(0);
+  
+}
+
+void getTurning(double speedMove, int dir, int turnTime){
+  //if dir = -1, turn right
+  getMoving(speedMove, dir, dir, turnTime);
+}
+
+void adjustAngle(double error, double _distanceBetweenWheels, double _wheelDiameter, double Kp){
+    double v = 0.0;
+    double w = Kp * error;
+    double leftWheelSpeed  =  v / (_wheelDiameter/2) - _distanceBetweenWheels * w / (_wheelDiameter) ;
+    double rightWheelSpeed  = v / (_wheelDiameter/2) + _distanceBetweenWheels * w / (_wheelDiameter) ;
+  
+    if (leftWheelSpeed > 250)
+      leftWheelSpeed = 250;
+    if (rightWheelSpeed > 250)
+      rightWheelSpeed = 250;
+    _leftMotor.run(leftWheelSpeed);
+    _rightMotor.run(-rightWheelSpeed);
+}