99 lines
1.8 KiB
C
99 lines
1.8 KiB
C
#include "io.h"
|
|
|
|
#define Ts 0.1
|
|
|
|
//float Kc = 1.0;
|
|
#define Ku 0.9
|
|
#define Tu 2.8
|
|
|
|
#define Kc 1.2//1//Ku/2.2 //=4.1
|
|
#define Ti 3//3//(2.2*Tu)
|
|
#define Td 0.5//0.5//(Tu/6.3)
|
|
|
|
const float p1 = (Kc*Ts)/Ti;
|
|
const float q1 = -Kc - (3*Kc*Td)/(2*Ts);
|
|
const float q2 = Kc + (7*Kc*Td)/(2*Ts);
|
|
const float q3 = -(5*Kc*Td)/(2*Ts);
|
|
const float q4 = (Kc*Td)/(2*Ts);
|
|
|
|
float y[4] = {0, 0, 0, 0};
|
|
float n = 0.0;
|
|
|
|
int controller(int dist, int r)
|
|
{
|
|
int i;
|
|
for (i=3; i>0; i--)
|
|
{
|
|
y[i] = y[i-1];
|
|
}
|
|
y[0] = dist;
|
|
|
|
n = n + p1 * ( r - y[1] ) + q1 * y[0] + q2 * y[1] + q3 * y[2] + q4 * y[3];
|
|
//n = Kc * ( r - y[0] );
|
|
|
|
if (n > 1000)
|
|
{
|
|
n = 1000;
|
|
}
|
|
else if (n < 0)
|
|
{
|
|
n = 0;
|
|
}
|
|
return (int)(n+0.5);
|
|
}
|
|
|
|
void *mainThread(void *arg0)
|
|
{
|
|
if (initUART() == false)
|
|
{
|
|
// initUart() failed
|
|
while(1);
|
|
}
|
|
// Samplefrequency = 10 Hz
|
|
if (initTimer(10) == false)
|
|
{
|
|
sendStringUART("initTimer() failed");
|
|
while(1);
|
|
}
|
|
|
|
initPort();
|
|
|
|
if (initPWM() == false)
|
|
{
|
|
sendStringUART("initPWM() failed");
|
|
while(1);
|
|
}
|
|
|
|
int i = 0, ii = -5, t = 300;
|
|
uint32_t sp = 0;
|
|
int PWM = 0;
|
|
char ploep[100];
|
|
|
|
//Kc = 2;
|
|
|
|
setPWM(0);
|
|
while(1)
|
|
{
|
|
trigger(); // Trigger routine creates a 10 usec trigger pulse for the ultrasonic sensor.
|
|
PWM = controller(distance, sp);
|
|
if(i < 30)
|
|
{
|
|
setPWM(0);
|
|
}
|
|
else
|
|
{
|
|
sp = 400;
|
|
setPWM(PWM);
|
|
}
|
|
|
|
sprintf(&ploep, "%d,%d,%d,%i\n", i, distance, sp, PWM);
|
|
sendStringUART(ploep);
|
|
|
|
i++;
|
|
while(tick == false){}
|
|
tick = false;
|
|
}
|
|
|
|
return 0;
|
|
}
|