57 lines
1.3 KiB
C++
57 lines
1.3 KiB
C++
#include "MeMCore.h"
|
|
#include <Wire.h>
|
|
const int Port_1 =9;
|
|
const int Port_2 =10;
|
|
|
|
MeDCMotor motor1(Port_1);
|
|
MeDCMotor motor2(Port_2);
|
|
MeUltrasonicSensor ultraSensor(PORT_3); /* Ultrasonic module can ONLY be connected to port 3, 4, 6, 7, 8 of base shield. */
|
|
uint8_t motorSpeed =100;
|
|
double distance;
|
|
double threshold= 10;
|
|
int Squarex[]={0,25,25,0};
|
|
int Squarey[]={0,0,25,25};
|
|
bool tmp=0;
|
|
double angle =0;
|
|
double angle2=0;
|
|
MeGyro gyro;
|
|
|
|
void setup()
|
|
{Serial.begin(9600);
|
|
gyro.begin();
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
if(tmp == 0){
|
|
motorSpeed =100;
|
|
motor1.run(-motorSpeed); /* value: between -255 and 255. */
|
|
motor2.run(motorSpeed); /* value: between -255 and 255. */
|
|
delay(2000);
|
|
motor1.stop();
|
|
motor2.stop();
|
|
tmp = 1;
|
|
}
|
|
|
|
if (tmp == 1) {
|
|
gyro.update();
|
|
|
|
Serial.read();
|
|
angle = 90 - gyro.getAngleZ();
|
|
Serial.println(gyro.getAngleZ());
|
|
|
|
motor1.run(2.8*angle); /* value: between -255 and 255. */
|
|
motor2.run(2.8*angle); /* value: between -255 and 255. */
|
|
|
|
if (2.8*angle <=100){
|
|
|
|
motor1.run(100); /* value: between -255 and 255. */
|
|
motor2.run(100);}
|
|
|
|
if(abs(2.8*angle) <= 35){
|
|
tmp =0;}
|
|
}
|
|
|
|
|
|
}
|