Merge branch 'develop'

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

@@ -0,0 +1,165 @@
+#include <MeMCore.h>
+#include <Wire.h>
+
+MeDCMotor _rightMotor(10);
+MeDCMotor _leftMotor(9);
+MeUltrasonicSensor mySensor(2);
+MeGyro myGyro;
+
+// Vars to change setup
+double Kp = -5;
+double target = 0;
+int changeTheta;
+int mSpeed = 150; //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, 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
+
+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:
+
+  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];
+    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));
+      //checking if the Z axis of the robot is aligned witht the theroretical axis
+    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;
+    }
+    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 + changeTheta;
+      if (target > 180){
+        myGyro.begin();
+        target = -180 + target;
+        }
+      if (target < -180){
+        myGyro.begin();
+        target = target +180;
+      }
+    }
+  }
+  
+}
+
+
+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);
+}