61 lines
920 B
C++
61 lines
920 B
C++
#include "Arduino.h"
|
|
#include "Wire.h"
|
|
#include "MeMCore.h"
|
|
#include "SoftwareSerial.h"
|
|
#include <Wire.h>
|
|
|
|
MeDCMotor rightMotor(10);
|
|
|
|
MeDCMotor leftMotor(9);
|
|
|
|
MeGyro gyro;
|
|
|
|
boolean x = true;
|
|
float Zzero = 0;
|
|
|
|
void setup() {
|
|
Serial.begin(9600);
|
|
rightMotor.run(0);
|
|
leftMotor.run(0);
|
|
|
|
gyro.begin();
|
|
|
|
}
|
|
|
|
void loop() {
|
|
|
|
gyro.update();
|
|
|
|
delay(3000);
|
|
|
|
|
|
int squareCoordonates[4][2]={{0,0},{25,0},{25,25},{0,25}};
|
|
|
|
for (int i=0; i<4; i++){
|
|
|
|
x = true;
|
|
|
|
while(x==true){
|
|
leftMotor.run(100);
|
|
rightMotor.run(-100);
|
|
delay(2150);
|
|
x=false;
|
|
}
|
|
float targetAngle = 90;
|
|
float Zzero=gyro.getAngleZ();
|
|
float changingangle=gyro.getAngleZ();
|
|
while (abs(Zzero-changingangle)< targetAngle){
|
|
Serial.print(gyro.getAngleZ());
|
|
leftMotor.run(100);
|
|
rightMotor.run(100);
|
|
gyro.update();
|
|
changingangle=gyro.getAngleZ();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|