Merge branch 'develop'

control/control.ino

@@ -0,0 +1,56 @@
+#include "Arduino.h"
+#include "Wire.h"
+#include "MeMCore.h"
+#include "SoftwareSerial.h"
+
+MeDCMotor _leftMotor(9);
+MeDCMotor _rightMotor(10);
+MeUltrasonicSensor ultrasonicSensor(PORT_3);
+MeGyro gyro;
+
+double dis;
+double ang;
+double oldAng;
+
+void setup() {
+  // put your setup code here, to run once:
+  Serial.begin(9600);
+  gyro.begin();
+  _leftMotor.run(0);
+  _rightMotor.run(0);
+  
+
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  
+  for (int i=0; i!=4 ;i++){
+    dis=ultrasonicSensor.distanceCm();
+    while (ultrasonicSensor.distanceCm()>dis-30){
+      _leftMotor.run(-100);
+      _rightMotor.run(100);
+      //Serial.println(ultrasonicSensor.distanceCm());
+    }
+    _leftMotor.stop();
+    _rightMotor.stop();
+    delay(1000);
+    ang=gyro.getAngleZ();
+    oldAng=gyro.getAngleZ();
+    while (abs(ang-gyro.getAngleZ())<90){
+      _leftMotor.run(-100);
+      _rightMotor.run(-100);
+      gyro.update();
+      oldAng=gyro.getAngleZ();
+    }
+    _leftMotor.stop();
+    _rightMotor.stop();
+    delay(1000);
+   
+  }
+  
+  delay(5000);
+  
+  
+
+}