kopia lustrzana https://github.com/YohanHadji/R2Home
16 wiersze
577 B
C++
16 wiersze
577 B
C++
// This file is used as an "add on", the only function defined here should be "gps_location where to go(float gps_latitude, float gps_longitude)"
|
|
// This function takes in parameter the current position, and should return a waypoint goal, which of course should be choosen based on the current position.
|
|
|
|
#include <PID_v1.h>
|
|
#include "config.h"
|
|
|
|
double Setpoint, Input, Output;
|
|
PID myPID(&Input, &Output, &Setpoint,NKP, NKI, NKD, DIRECT);
|
|
|
|
float cmpt_cmd(float err) {
|
|
Input = -err;
|
|
Setpoint = 0;
|
|
myPID.Compute();
|
|
return map(Output, -180, 180, 1000, 2000);
|
|
}
|