82 lines
1.3 KiB
C++
82 lines
1.3 KiB
C++
|
|
#include "I2Cdev.h"
|
|
|
|
|
|
#include <Servo.h>
|
|
Servo left;
|
|
Servo right;
|
|
|
|
float angleOffset = -2.4241745;
|
|
float desiredYaw = 0.0;
|
|
float currentYaw = 0.0;
|
|
|
|
struct DebugState{
|
|
int motorTargetAngle;
|
|
};
|
|
|
|
DebugState dbgState;
|
|
|
|
#include "distance.h"
|
|
#include "encoder.h"
|
|
#include "gyro.h"
|
|
#include "pid.h"
|
|
#include "webserver.h"
|
|
|
|
|
|
void initSerial(){
|
|
Serial.begin(115200);
|
|
while (!Serial);
|
|
delay(300);
|
|
Serial.print("\n\n\n\n\n\n\n\n\n\n\n");
|
|
}
|
|
|
|
void initMotors(){
|
|
left.attach(D3);
|
|
right.attach(D4);
|
|
}
|
|
|
|
void initI2C(){
|
|
digitalWrite(D2, LOW);
|
|
digitalWrite(D1, LOW);
|
|
delay(100);
|
|
Wire.begin();
|
|
Wire.setClock(400000);
|
|
}
|
|
|
|
void setup() {
|
|
initSerial();
|
|
initWifi(false);
|
|
initServer();
|
|
initMotors();
|
|
initPID();
|
|
initI2C();
|
|
initEncoder();
|
|
initDistance();
|
|
initGyro();
|
|
}
|
|
|
|
void loop() {
|
|
if (updateGyro()) { //gyro data
|
|
currentYaw=ypr[0]*180/M_PI;
|
|
double angle = ypr[1] * 180 / M_PI;
|
|
if(angle>180)
|
|
angle -= 180;
|
|
angleInput = angle + angleOffset;
|
|
}
|
|
{// encoder data
|
|
updateEncoder();
|
|
posInput = encoder.position();
|
|
}
|
|
updateDistance();
|
|
|
|
|
|
Speeds speeds = updatePID();
|
|
|
|
speeds.left = min(90.0f-10, max(-90.0f+10, speeds.left));
|
|
speeds.right = min(90.0f-10, max(-90.0f+10, speeds.right));
|
|
left.write(90+(int)speeds.left);
|
|
right.write(90+(int)speeds.right);
|
|
|
|
delay(5);
|
|
}
|