57 lines
1.0 KiB
C++
57 lines
1.0 KiB
C++
#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);
|
|
|
|
|
|
|
|
}
|