64 lines
1.7 KiB
C++
64 lines
1.7 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.0;
|
|
double angleDifference=0.0;
|
|
double targetAngle = 90.0;
|
|
|
|
MeGyro gyro;
|
|
|
|
void setup()
|
|
{
|
|
Serial.begin(9600);
|
|
gyro.begin();
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
if(tmp == 0){ /* if-loop with flag to make the robot go 25 cm forward */
|
|
motorSpeed = 100;
|
|
motor1.run(-motorSpeed); /* value: between -255 and 255. */
|
|
motor2.run(motorSpeed); /* value: between -255 and 255. */
|
|
delay(2000);
|
|
motor1.stop();
|
|
motor2.stop();
|
|
gyro.update();
|
|
angle = gyro.getAngleZ(); /* Sense the initial position of the robot before turning */
|
|
tmp = 1; /* update flag */
|
|
}
|
|
|
|
if (tmp == 1) { /* if loop for rotation after translation */
|
|
|
|
gyro.update();
|
|
angleDifference = gyro.getAngleZ() - angle; /* Calculate how much the robot turned */
|
|
|
|
if(angleDifference < targetAngle){
|
|
if (angleDifference < 0){
|
|
motorSpeed = 2.83*(targetAngle);
|
|
}
|
|
else{
|
|
motorSpeed = 2.83*(targetAngle-angleDifference);
|
|
}
|
|
motor1.run(motorSpeed);
|
|
motor2.run(motorSpeed);
|
|
}
|
|
|
|
if(angleDifference >= targetAngle){ /* if robot reaches 90°, stop rotation and update flag*/
|
|
motor1.stop();
|
|
motor2.stop();
|
|
tmp = 0;
|
|
}
|
|
}
|
|
}
|