Adding the control file that is supposed to make the robot make a square of 25cm.
This commit is contained in:
parent
80864c4f14
commit
536d2a955c
|
|
@ -0,0 +1,60 @@
|
||||||
|
#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();
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue