removed old/ obsolete pid_orig.c
This commit is contained in:
tims
@@ -1,128 +0,0 @@
|
|||||||
/*
|
|
||||||
* pid.c
|
|
||||||
* Created on: Jan 28, 2019
|
|
||||||
* Author: laurenzb
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "xparameters.h"
|
|
||||||
#include "ucos_uartps.h"
|
|
||||||
#include "xil_printf.h"
|
|
||||||
#include "ucos_ii.h"
|
|
||||||
#include "ucos_bsp.h"
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <math.h>
|
|
||||||
#include "pid.h"
|
|
||||||
|
|
||||||
|
|
||||||
#define PRINTPID 0
|
|
||||||
|
|
||||||
#define KP 300.0
|
|
||||||
#define KD 0.0
|
|
||||||
#define KI 200.0
|
|
||||||
#define SAMPLE_TIME ((float)(5.0/1000.0))
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
static float targetAngle = 0.0;
|
|
||||||
extern int16_t accData[3];
|
|
||||||
extern int16_t gyroData[3];
|
|
||||||
int16_t gyroOffset[3];
|
|
||||||
|
|
||||||
//int16_t accMin[3] = {-8960, -9560, -8767}; //gelb
|
|
||||||
//int16_t accMax[3] = {9077, 9010, 9429}; //gelb
|
|
||||||
|
|
||||||
//int16_t accMin[3] = {-7890, -8277, -9915}; //braun
|
|
||||||
//int16_t accMax[3] = {9870, 8983, 8420}; //braun
|
|
||||||
|
|
||||||
int16_t accMin[3] = {-8757 , -8649 , -8547}; //gruen
|
|
||||||
int16_t accMax[3] = {9221 , 9725 , 9158}; //gruen
|
|
||||||
|
|
||||||
//int16_t accMin[3] = {-8554 ,-8513 , -8341}; //rot
|
|
||||||
//int16_t accMax[3] = {8862 ,8385 ,9283}; //rot
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
int32_t pidValue=0;
|
|
||||||
float errorSumAngle=0;
|
|
||||||
float prevAngle=0;
|
|
||||||
float gyroAnglePerStep=0;
|
|
||||||
int cycle = 0;
|
|
||||||
|
|
||||||
/* * * * * * * * calculates a PID value from -1000 to 1000 by using acc and gyro data * * * * * * * * */
|
|
||||||
int pid_Task(void* pdata) {
|
|
||||||
mpu9250_Get_Acc_Min_Max();
|
|
||||||
cycle++;
|
|
||||||
if (cycle == 10 * 1000/5)
|
|
||||||
targetAngle = prevAngle;
|
|
||||||
|
|
||||||
float accAngle=0, currentAngle=0, errorAngle=0, gyroRate=0;
|
|
||||||
int16_t accAxisOne = map(pid_Constrain(accData[0], accMin[0], accMax[0]), accMin[0], accMax[0],-32767, 32767);
|
|
||||||
int16_t accAxisTwo = map(pid_Constrain(accData[2], accMin[2], accMax[2]), accMin[2], accMax[2],-32767, 32767);
|
|
||||||
accAngle = (float)atan2((double)(accAxisOne), (double)(accAxisTwo))*(180/PI); //calc the angle of inclination only by using accelerometer data
|
|
||||||
accAngle = -accAngle; //change the orientation
|
|
||||||
gyroRate = (((float)(gyroData[1]+gyroOffset[1]))/(65.5)); //calculate real gyro value according to the set sensivity, for more info see datasheet
|
|
||||||
gyroAnglePerStep = gyroRate*SAMPLE_TIME; //change to last step, calculated by gyro
|
|
||||||
currentAngle = 0.98*(prevAngle + gyroAnglePerStep) + 0.02*accAngle; //complementary filter, eleminates noise from the acc and fixes gyro drift
|
|
||||||
errorAngle = currentAngle - targetAngle; //calc the error to the disired angle
|
|
||||||
errorSumAngle = errorSumAngle + errorAngle; //integrate the error to use it in the PID
|
|
||||||
errorSumAngle = (float)pid_Constrain((int)errorSumAngle, -750, 750); //constrain maximum integrated error
|
|
||||||
|
|
||||||
CPU_SR cpu_sr;
|
|
||||||
OS_ENTER_CRITICAL();
|
|
||||||
pidValue = (int16_t)(KP*(errorAngle) + KI*(errorSumAngle)*SAMPLE_TIME + KD/SAMPLE_TIME*(currentAngle-prevAngle)); //calculate output from P, I and D values
|
|
||||||
pidValue = pid_Constrain(pidValue, -PID_BOUND, PID_BOUND); //constrain maximum integrated error, to use the max value in calculations for step frequency
|
|
||||||
|
|
||||||
if(pidValue == 0){ //prevent divison by 0
|
|
||||||
pidValue=1;
|
|
||||||
}
|
|
||||||
OS_EXIT_CRITICAL();
|
|
||||||
prevAngle = currentAngle; //prepare next iteration
|
|
||||||
|
|
||||||
if (PRINTPID==1){
|
|
||||||
UCOS_Print("Angle:");
|
|
||||||
UCOS_Printf("%8d ", (int)currentAngle);
|
|
||||||
|
|
||||||
UCOS_Print("Acc:");
|
|
||||||
UCOS_Printf("%8d ", (int)accAngle);
|
|
||||||
|
|
||||||
//UCOS_Print("Pid:");
|
|
||||||
//UCOS_Printf("%8d ", (int)pidValue);
|
|
||||||
|
|
||||||
UCOS_Printf("\r\n");
|
|
||||||
}
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* * * * * * * * clears Gyro offset and sets it new * * * * * * * * */
|
|
||||||
unsigned int pid_Init(void *pdata){
|
|
||||||
uint8_t i = 0;
|
|
||||||
uint8_t t = 0;
|
|
||||||
for (i = 0; i < 3; i++) {
|
|
||||||
gyroOffset[i]=0;
|
|
||||||
}
|
|
||||||
for (t = 0; t <10; t++) {
|
|
||||||
for (i = 0; i < 3; i++) {
|
|
||||||
gyroOffset[i]=(gyroData[i]/10);
|
|
||||||
}
|
|
||||||
OSTimeDly(OS_TICKS_PER_SEC/10);
|
|
||||||
}
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* * * * * * * * sets outer bounds * * * * * * * * */
|
|
||||||
int16_t pid_Constrain(int16_t value, int16_t lowerBorder, int16_t higherBorder){
|
|
||||||
if (value > higherBorder){
|
|
||||||
value = higherBorder;
|
|
||||||
}
|
|
||||||
if (value < lowerBorder){
|
|
||||||
value = lowerBorder;
|
|
||||||
}
|
|
||||||
return value;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* * * * * * maps to another numberroom * * * * * * */
|
|
||||||
int16_t map(int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max) {
|
|
||||||
return ((x - in_min) * (out_max - out_min)) / (in_max - in_min) + out_min;
|
|
||||||
}
|
|
||||||
Reference in New Issue
Block a user