main.cpp

00001 #include "mbed.h"
00002 const double Ts = // sample frequency 
00003 
00004 //PID rotation - aanmaken van PID constanten en variabelen voor rotatie
00005 const double = Rot_Kp =  , Rot_Ki =  , Rot_Kd = ;
00006 double Rot_error = 0;
00007 double Rot_prev_error = 0;
00008 
00009 //PID translation - aanmaken van PID constanten en variabelen voor translatie
00010 const double = Trans_Kp =  , Trans_Ki =  , Trans_Kd = ;
00011 double Trans_error = 0;
00012 double Trans_prev_error = 0;
00013 
00014 // PID execution
00015 double PID_control(double error, const double kp, const double ki, const double kd, 
00016 { 
00017 // P control
00018 double u_k = pk * error;
00019 
00020 // I control
00021 error_int = error_int + (Ts * error);
00022 double u_i = ki * error_int;
00023 
00024 // D control
00025 double error_deriv = (error - error_prev);
00026 double u_d = kd * error_deriv;
00027 error_prev = error;
00028 
00029 return u_k + u_i + u_d;
00030 
00031 int main()
00032 {
00033 
00034 }