68 lines
1.4 KiB
C++
68 lines
1.4 KiB
C++
#include "MeMCore.h"
|
|
#include <Wire.h>
|
|
|
|
MeDCMotor motor1(9);
|
|
MeDCMotor motor2(10);
|
|
MeGyro gyro(PORT_4);
|
|
|
|
const uint8_t motorSpeed = 100;
|
|
const unsigned long forwardDuration = 2000;
|
|
const float Kp = 5.0;
|
|
|
|
void setup() {
|
|
Serial.begin(9600);
|
|
gyro.begin();
|
|
}
|
|
|
|
void turnRight90Degrees() {
|
|
float targetAngle = ((int)gyro.getAngleZ() - 90)%360;
|
|
float error;
|
|
do {
|
|
gyro.update();
|
|
|
|
error = targetAngle - gyro.getAngleZ();
|
|
int speedAdjustment = Kp * error;
|
|
|
|
motor1.run(constrain(-motorSpeed + speedAdjustment, -motorSpeed, motorSpeed));
|
|
motor2.run(constrain(motorSpeed - speedAdjustment, motorSpeed, -motorSpeed));
|
|
Serial.print("gyro.getAngleZ()= ");
|
|
Serial.print(gyro.getAngleZ());
|
|
Serial.print("error= ");
|
|
Serial.print(error);
|
|
Serial.print("speedAdjustment= ");
|
|
Serial.println(speedAdjustment);
|
|
delay(50);
|
|
} while (abs(error) > 2); // Exit the loop when the error is less than 2 degrees
|
|
|
|
motor1.stop();
|
|
motor2.stop();
|
|
}
|
|
|
|
void loop() {
|
|
for (int i = 0; i < 4; i++) {
|
|
// Move forward
|
|
motor1.run(-motorSpeed);
|
|
motor2.run(motorSpeed);
|
|
delay(forwardDuration);
|
|
|
|
// Stop the motors
|
|
motor1.stop();
|
|
motor2.stop();
|
|
delay(1000);
|
|
|
|
// Turn right 90 degrees using the gyro sensor
|
|
turnRight90Degrees();
|
|
|
|
// Stop the motors
|
|
motor1.stop();
|
|
motor2.stop();
|
|
delay(1000);
|
|
|
|
}
|
|
//Stop the robot after drawing the square
|
|
while (true) {
|
|
motor1.stop();
|
|
motor2.stop();
|
|
}
|
|
}
|