ejung/sdes_lab
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

removed old/ obsolete pid_orig.c

Browse Source
This commit is contained in:
tims
2025-03-14 10:40:07 +01:00
parent 73c78242e9
commit e2e5b79c68
1 changed files with 0 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files

128
src/APP/Aufgabe7/ps7/core0/src/pid_orig.c
View File

@@ -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;
}