Merge branch 'develop'

control/control.ino

@@ -0,0 +1,74 @@
+#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;
+
+// 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
+
+void setup() {
+  // put your setup code here, to run once:
+  Serial.begin(115200);
+  _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:
+    error = target - _thetaCurrent;
+    //Serial.println(error);
+    if (error > 0.1){
+      adjustAngle(error, _distanceBetweenWheels, _wheelDiameter, Kp);
+    }
+    myGyro.update();
+    _thetaCurrent = myGyro.getAngleZ() - _thetaBaseline;
+    error = target - _thetaCurrent;
+
+  
+}
+
+
+void getMoving(double speedMove, int dirRight, int dirLeft){
+  //if direction 1 -1 forward
+  _leftMotor.run(dirLeft*speedMove);
+  _rightMotor.run(dirRight*speedMove);
+}
+
+void getTurning(double speedMove, int dir){
+  //if dir = -1, turn right
+  getMoving(speedMove, dir, dir);
+}
+
+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);
+}

gyro/gyro.ino

@@ -0,0 +1,21 @@
+#include <MeMCore.h>
+#include <Wire.h>
+
+MeGyro myGyro;
+
+void setup() {
+  // put your setup code here, to run once:
+  Serial.begin(9600);
+  myGyro.begin();
+  //myGyro.setpin();
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  myGyro.update();
+  Serial.print("Z axis gyro ");
+  Serial.println(myGyro.getAngleZ());
+  Serial.print("Y axis gyro ");
+  Serial.println(myGyro.getAngleY());
+
+}

roomba/roomba.ino

@@ -17,7 +17,7 @@ void setup() {
 void loop() {
   // put your main code here, to run repeatedly:
   getMoving(150, 1, -1, 500);
-  if (mySensor.distanceCm()< 20){
+  if (mySensor.distanceCm()< 25){
     _leftMotor.stop();
     _rightMotor.stop();
     valTurn = (int) random(0, 2)*2-1;