95 lines
1.9 KiB
C++
95 lines
1.9 KiB
C++
//#include "MeOrion.h"
|
|
#include <MeMCore.h>
|
|
#include <Wire.h>
|
|
|
|
MeGyro gyro;
|
|
MeDCMotor motor3(M1);
|
|
MeDCMotor motor4(M2);
|
|
|
|
uint8_t motorSpeed = 200;
|
|
MeUltrasonicSensor ultraSensor(PORT_3);
|
|
|
|
double rWheel = 6.5;
|
|
double dWheel = 11.5;
|
|
|
|
double Pos[4][1];
|
|
|
|
double angularError = 10;
|
|
double posError = 10;
|
|
|
|
double angle1;
|
|
double angle2;
|
|
double dist;
|
|
|
|
void setup()
|
|
{
|
|
Serial.begin(115200);
|
|
gyro.begin();
|
|
|
|
Pos[0][0] = 0;
|
|
Pos[0][1] = 0;
|
|
Pos[1][0] = 25;
|
|
Pos[1][1] = 0;
|
|
Pos[2][0] = 25;
|
|
Pos[2][1] = 25;
|
|
Pos[3][0] = 0;
|
|
Pos[3][1] = 25;
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
Serial.println("Start Loop");
|
|
int i;
|
|
int j;
|
|
Serial.read();
|
|
for(i=0;i<4;i++){
|
|
Serial.print("Beggin angular adjustment for i: ");
|
|
Serial.println(i);
|
|
//angle adjustment
|
|
gyro.update();
|
|
|
|
angle1=gyro.getAngleZ();
|
|
angle2=atan( (Pos[i+1][1]-Pos[i][1])/ (Pos[i+1][0]-Pos[i][0]) );
|
|
motorSpeed = 70;
|
|
Serial.print("angle1: ");
|
|
Serial.print(angle1);
|
|
Serial.print(" angle2: ");
|
|
Serial.println(angle2);
|
|
|
|
while(angle1-angle2+360 > angularError) {
|
|
gyro.update();
|
|
angle1=gyro.getAngleZ();
|
|
angle2=atan( (Pos[i+1][0]-Pos[i][0])/ (Pos[i+1][1]-Pos[i][1]) );
|
|
Serial.println("i: ");
|
|
Serial.println(i);
|
|
Serial.print(" angle1: ");
|
|
Serial.print(angle1);
|
|
Serial.print(" angle2: ");
|
|
Serial.println(angle2);
|
|
if(angle2+180<angle1+180){
|
|
motor3.run(-motorSpeed);
|
|
motor4.run(-motorSpeed);
|
|
} else {
|
|
motor3.run(motorSpeed);
|
|
motor4.run(motorSpeed);
|
|
}
|
|
delay(200);
|
|
}
|
|
motor3.stop();
|
|
motor4.stop();
|
|
//translation
|
|
dist=sqrt( pow(Pos[i+1][0]-Pos[i][0],2)+pow(Pos[i+1][1]-Pos[i][1],2) );
|
|
motorSpeed = 0.2/rWheel;
|
|
for(j = 0;j<5;j++) {
|
|
motor3.run(motorSpeed);
|
|
motor4.run(-motorSpeed);
|
|
delay(1000);
|
|
}
|
|
|
|
motor3.stop();
|
|
motor4.stop();
|
|
delay(1000);
|
|
}
|
|
delay(100);
|
|
}
|