#include

#include
#include
#include
#include
#include

<Wire.h>
"IMU.h"
"tools.h"
"filters.h"
"Encoders.h"
"mario.h"

#define
#define
#define
#define

MPW_R_PIN
MDR_R_PIN
MPW_L_PIN
MDR_L_PIN

10
12
11
13

#define
#define
#define
#define

EN_R0_PIN
EN_R1_PIN
EN_L0_PIN
EN_L1_PIN

A0
A1
A2
A3

//ENCODER myEncoder;
//PID_CONTROL PID_MTR_R;
//PID_CONTROL PID_MTR_L;
PID_CONTROL PID_MTR_pitch(50, 0, 5);
PID_CONTROL PID_MTR_fall(10, 0, 2);
PID_CONTROL PID_MTR_yaw(3, 0, 1);
PID_CONTROL PID_MTR_twist(1, 0, 0);
TIMER_MS arming_timer;

double angCor_Kp = 0.3;

double angCor_Kd = 0.8;
double speed_x = 0;
double direction_y = 0;
#define echoTimeOut 12000
unsigned long Sonar_time;
double US_read;
bool US_readed = false;
FILTER US_filter;
bool AutoMode = false;
bool playMario = false;
void setup() {
Serial.begin(115200);
imu_init();
//myEncoder.init();
pinMode(4, OUTPUT);
attachInterrupt(0, US_echo, FALLING);
}
double yaw;
double pitch;

double
FILTER
double
double
FILTER

desire_tilt;
pitchFilter;
estSpeedErr = 0;
estSpeedErr_old = 0;
estSpeedFilter;

long int time_step;

double anlgeCorrect;
bool armed = false;
TIMER_MS _100Hz_loop;
uint8_t loop_count;
void loop() {
//fast loop
serialRead();
readArmingSwitch();
if (playMario)mario();
//myEncoder.readWheels();
if (_100Hz_loop.tac() > 10) {
_100Hz_loop.tic();
loop_count++; // loop counter for sub-loop
if (loop_count > 100)loop_count = 0; // reset counter when it hit 100, 100*1
0ms = 1s
//100Hz loop
imu_read(); //Read IMU with 100Hz sample rate
if (abs(gx) >= 32767) Serial.println("Angular Speed out of range!");
double dt = double(micros() - time_step) / 1000; //converting to Millisecond
for PID control
time_step = micros();
// via accelerometer
double accPitch = (atan2(-ay, -az) + PI) * RAD_TO_DEG;
accPitch = constrain_angle_deg(accPitch);
// integrating angular speed via gyro
double rotout = (abs(gz) > 150) ? double(gz) / 32767.0f * 0.25 : 0;
double tltout = (abs(gx) > 150) ? double(gx) / 32767.0f * 0.25 : 0;
yaw += rotout * dt;
yaw = constrain_angle_deg(yaw);
// complementary filtering
double tau = 0.075;
double a = 0.0;
a = tau / (tau + dt / 1000);
pitch = a * (pitch + tltout * dt) + (1 - a) * (accPitch);
//pitch = pitchFilter.Complementary(accPitch, pitch, dt);u
double forword = PID_MTR_pitch.output(desire_tilt, pitch, dt) + PID_MTR_fall
.output(0, tltout * 1000, dt);
forword = constrain(forword, -255, 255);

//Mforword = constrain_map_255(forword);
double angleDiff = yaw - direction_y;
angleDiff = constrain_angle_deg(angleDiff);
double turn = PID_MTR_yaw.output(0, angleDiff, dt) + PID_MTR_twist.output(0,
rotout * 500, dt);

//
//Speed estimation
estSpeedErr += //estSpeedFilter.fwrdAvg_sigma
(forword - speed_x);

//

estSpeedErr_old = estSpeedErr;

if (loop_count % 10 == 0) //10Hz loop

{
estSpeedErr *= 0.0001;
desire_tilt += estSpeedErr * angCor_Kp;
desire_tilt += (estSpeedErr - estSpeedErr_old) * angCor_Kd;
desire_tilt = constrain(desire_tilt, -20, 20);
Serial.println(desire_tilt);
US_triger(4);
double dist = US_filter.fwrdAvg_sigma(US_read);
if (AutoMode) {
if (dist < 190) {
speed_x = 0;
if (abs(angleDiff) < 3) {
Serial.println("Set angle");
direction_y -= 45;
direction_y = constrain_angle_deg(direction_y);
}
}
else speed_x = 45;
}
}
if (loop_count % 100 == 0) //1Hz loop
{
}
motor_power(forword + turn, forword - turn); // final stage, writing motor v
alues to motors
if (_100Hz_loop.tac() > 10) Serial.println("Waring: Tasks run over time.");
}
}
void readArmingSwitch() {
if (analogRead(A1) < 512)arming_timer.tic();
if (arming_timer.tac() > 1000) {
toggle_arming();
arming_timer.tic();

}
}
double constrain_angle_deg(double angle) {
angle = (angle > 180) ? (angle - 360) : ((angle < -180) ? (angle + 360) : angl
e);
return angle;
}
double constrain_map_255(double val) {
double in_min, in_max;
double out_min, out_max;
if (val < 0)
{
in_min = val;
in_max = 0;
out_min = (val < -255) ? -255 : val;
out_max = 0;
}
else
{
in_min = 0;
in_max = val;
out_min = 0;
out_max = (val > 255) ? 255 : val;
}
val = map(val, in_min, in_max, out_min, out_max);
return val;
}
void toggle_arming() {
armed = !armed;
anlgeCorrect = 0;
direction_y = yaw;
Serial.println((armed) ? "Armed" : "Disarmed");
}
void serialRead() {
if (Serial.available() > 0) {
// read the incoming byte:
char incomingByte = Serial.read();
switch (incomingByte) {
case 'w':
speed_x = 45;
Serial.println("forward");
break;
case 's':
speed_x = -45;
Serial.println("backward");
break;
case 'd':
direction_y -= 45;
direction_y = (direction_y >= 360) ? direction_y - 360 : ((direction_y <
0) ? direction_y + 360 : direction_y);
Serial.println("turn left");
break;
case 'a':

direction_y += 45;
direction_y = (direction_y >= 360) ? direction_y - 360 : ((direction_y <
0) ? direction_y + 360 : direction_y);
Serial.println("turn right");
break;
case '-':
readOption();
break;
case ' ':
speed_x = 0;
break;
}
}
}
void readOption() {
if (Serial.available() > 1) {
char opt = Serial.read();
char sp = Serial.read();
if (sp != ' ') {
Serial.println("Error, can't read option");
return;
}
switch (opt) {
case 'P':
Serial.print("Set Kp:");
Serial.print(PID_MTR_pitch.Kp = readValue());
break;
case 'I':
Serial.print("Set Ki:");
//Serial.print(PID_MTR_pitch.Ki=readValue());
break;
case 'D':
Serial.print("Set Kd:");
Serial.print(PID_MTR_pitch.Kd = readValue());
break;
case 'p':
Serial.print("Set Kp:");
Serial.print(angCor_Kp = readValue());
break;
case 'i':
Serial.print("Set Ki:");
//Serial.print(PID_MTR_pitch.Ki=readValue());
break;
case 'd':
Serial.print("Set Kd:");
Serial.print(angCor_Kd = readValue());
break;
case 'b':
Serial.print("Bat Lvl: ");
Serial.print(float(analogRead(A0)) / 1024.0f * 5);
break;
case 'N':
AutoMode = !AutoMode;
Serial.print("AutoMode:");
Serial.print(AutoMode ? "No" : "Off");
break;
case 'm':
playMario = !playMario;

playTime = millis();
Serial.print("Mario:");
Serial.print(playMario ? "No" : "Off");
break;
default:
Serial.print("Option not fond");
}
Serial.println();
}
else Serial.println("Error, can't read option");
}
float readValue() {
String val = "";
if (Serial.available() > 0) {
while (Serial.available() > 0) {
char inChar = Serial.read();
if (inChar == ' ') break;
val += inChar;
}
//val.trim();
//Serial.print(val.toFloat());
return val.toFloat();
}
else Serial.println("Error, can't read value");
}
void motor_power(int PW_R, int PW_L)
{
if (!armed) {
analogWrite(MPW_R_PIN, 0);
analogWrite(MPW_L_PIN, 0);
return;
}
digitalWrite(MDR_R_PIN, (PW_R > 0) ? LOW : HIGH);
digitalWrite(MDR_L_PIN, (PW_L > 0) ? LOW : HIGH);
//
//
//
//
//
//
//
//
//

PW_R = abs(PW_R);
PW_L = abs(PW_L);
if (int max = max(PW_L, PW_R) > 255) {
PW_R = map(PW_R, 0, max, 0, 255);
PW_R = map(PW_R, 0, max, 0, 255);
}
analogWrite(MPW_R_PIN, PW_R);
analogWrite(MPW_L_PIN, PW_L);

analogWrite(MPW_R_PIN, constrain(abs(PW_R), 0, 255));

analogWrite(MPW_L_PIN, constrain(abs(PW_L), 0, 255));
}
void US_triger(int pin) {
if ((micros() - Sonar_time > echoTimeOut)) US_readed = true;
if (!US_readed) return;
digitalWrite(pin, LOW);
delayMicroseconds(2);

digitalWrite(pin, HIGH);
delayMicroseconds(10);
digitalWrite(pin, LOW);
US_readed = false;
Sonar_time = micros();
}
void US_echo() {
if (US_readed) return;
unsigned long dt = micros() - Sonar_time;
US_read = (dt <= echoTimeOut) ? (double)dt : (double)echoTimeOut;
US_read *= 0.17;
US_readed = true;
}