79 lines
980 B
C++
79 lines
980 B
C++
#include "MeMCore.h"
|
|
#include <Wire.h>
|
|
|
|
MeGyro gyro;
|
|
MeDCMotor leftMotor(9);
|
|
MeDCMotor rightMotor(10);
|
|
|
|
uint8_t motorSpeed = 100;
|
|
|
|
void setup() {
|
|
|
|
Serial.begin(115200);
|
|
gyro.begin();
|
|
|
|
leftMotor.run(0);
|
|
rightMotor.run(0);
|
|
|
|
}
|
|
|
|
void loop() {
|
|
//(0,0)
|
|
gyro.update();
|
|
goForward();
|
|
delay(2000);
|
|
stop();
|
|
|
|
//(10,0)
|
|
|
|
float error = 100;
|
|
float d_target = 90;
|
|
float p = 0.5;
|
|
float v_e;
|
|
float v_r;
|
|
|
|
while (error > 10){
|
|
gyro.update();
|
|
error = d_target - gyro.getAngleZ();
|
|
v_e = p*error;
|
|
v_r = -v_e;
|
|
goLeft(v_r);
|
|
}
|
|
stop();
|
|
|
|
//(10,10)
|
|
//(0,10)
|
|
|
|
}
|
|
|
|
void goForward(){
|
|
|
|
leftMotor.run(-motorSpeed);
|
|
rightMotor.run(motorSpeed);
|
|
delay(1000);
|
|
|
|
}
|
|
|
|
void goBackward(){
|
|
|
|
leftMotor.run(motorSpeed);
|
|
rightMotor.run(-motorSpeed);
|
|
delay(100);
|
|
|
|
}
|
|
|
|
void goLeft(float rotateSpeed){
|
|
|
|
leftMotor.run(rotateSpeed);
|
|
rightMotor.run(rotateSpeed);
|
|
delay(100);
|
|
|
|
}
|
|
|
|
void stop(){
|
|
|
|
leftMotor.stop();
|
|
rightMotor.stop();
|
|
delay(100);
|
|
|
|
} |