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

Updated Aufgabe7

Browse Source
This commit is contained in:
Kai-Bjoern Gemlau
2020-05-29 12:04:04 +02:00
parent 7f950d9ccf
commit 5c7125398d
4 changed files with 92 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/APP/Aufgabe7/ps7/core0/cfg/pid.h
View File

@@ -8,6 +8,14 @@
#ifndef SRC_APP_AUFGABE7_PS7_CORE0_CFG_PID_H_
#define SRC_APP_AUFGABE7_PS7_CORE0_CFG_PID_H_
#define PID_BOUND 1000 //Restrict PID Value in [-1000,1000]
#define PI 3.14159265
int pid_Init(void *pdata);
int16_t pid_Constrain(int16_t value, int16_t lowerBorder, int16_t higherBorder);
int pid_Task(void *pdata);
int16_t map(int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max);
#endif /* SRC_APP_AUFGABE7_PS7_CORE0_CFG_PID_H_ */

1
src/APP/Aufgabe7/ps7/core0/src/app_hooks.c
View File

@@ -216,6 +216,7 @@ void App_TaskSwHook (void)
#if (APP_CFG_PROBE_OS_PLUGIN_EN > 0) && (OS_PROBE_HOOKS_EN > 0)
OSProbe_TaskSwHook();
#endif
GT_TaskSw();
}
#endif

75
src/APP/Aufgabe7/ps7/core0/src/gt_tasks.c
View File

@@ -2,8 +2,7 @@
* gt_tasks.c
*
* Created on: Aug 22, 2014
* Author: matthiasb
*
* Author: matthiasb
*/
#include "ucos_uartps.h"
@@ -12,96 +11,44 @@
#include "ucos_bsp.h"
#include "gt_types.h"
#include "gt_cfg.h"
#include <stdlib.h> /* rand */
#include <string.h> /* memset */
#include "ttc_timer.h"
#include "imu.h"
#include "pid.h"
/*-------------------------------Task model internal struct definitions-------------------------------
* Internal task extension struct
*/
typedef struct
{
CPU_INT32U BCET;
CPU_INT32U WCET;
}GT_WL_CET_T;
/*
* Internal task extension struct
*/
typedef struct
{
CPU_INT32U Period;
CPU_INT32U Jitter;
CPU_INT32U Distance;
GT_WL_CET_T CET;
}GT_TASK_EXT_T;
/*------------------------------------Execution times and periods-------------------------------------*/
/*
* All times given in (1/10)ms
*/
/*
* Task model
*/
GT_TASK_EXT_T GT_AllTasks[GT_NUM_OF_TASKS] = {
/* P J D BCET WCET */
/* Period Jitter Distance BCET WCET */
{50, 0, 0, {1, 1}},
{120, 0, 0, {2, 4}},
{50, 0, 0, {3, 5}},
//{50, 0, 0, {3, 5}},
};
/*------------------------------------Internal call back functions-------------------------------------*/
/*
* Function to determine task execution time
*/
static CPU_INT32U _GT_GetTaskCet(GT_TASK_T * pThisTask)
{
/*
* Simple randomized jitter between BCET and WCET
*/
GT_TASK_EXT_T * pTaskExt = (GT_TASK_EXT_T*)pThisTask->Extension;
CPU_INT32U BCET = pTaskExt->CET.BCET;
CPU_INT32U WCET = pTaskExt->CET.WCET;
return (BCET < WCET) ? (WCET - (rand() % (WCET - BCET))) : WCET;
}
/*
* Function to determine next task activation
* For this model we use a simple PJD model
*/
static CPU_INT32U _GT_GetNextActivation(GT_TASK_T * pThisTask)
{
GT_TASK_EXT_T * pTaskExt = (GT_TASK_EXT_T*)pThisTask->Extension;
/*
* Calculate next activation based on PJ Model
*/
CPU_INT32S NextDistance = (pTaskExt->Jitter) ? pTaskExt->Period + (rand()%(pTaskExt->Jitter) - (pTaskExt->Jitter/2))
: pTaskExt->Period;
/*
* Adjust activation to PJD if needed
*/
return ((CPU_INT32S)pTaskExt->Distance > NextDistance) ? pTaskExt->Distance : (CPU_INT32U)NextDistance;
}
static CPU_INT32U _GT_ComStackFkt(GT_TASK_T * pThisTask){
}
/*
* Main task table, used for GT_Init call
*/
GT_TASK_T GT_Tasks[GT_NUM_OF_TASKS] =
{
/*Type Activation Type Id Prio Next Activation CET Task init Function Task Function TaskArg D WL/Runable Internal External */
{ GT_TASK_EXT , GT_ACT_INT , 0, 32, _GT_GetNextActivation, _GT_GetTaskCet, your-timer-taskinitfunction ,your-timer_Task , NULL, 50, GT_RUNABLE_NULL, GT_INTERNAL_NULL, (void *)&GT_AllTasks[0]},
{ GT_TASK_EXT , GT_ACT_INT , 1, 31, _GT_GetNextActivation, _GT_GetTaskCet, your-imu-taskinitfunction ,your-imu_Task , NULL, 50, GT_RUNABLE_NULL, GT_INTERNAL_NULL, (void *)&GT_AllTasks[1]},
{ GT_TASK_EXT , GT_ACT_INT , 2, 33, _GT_GetNextActivation, _GT_GetTaskCet, your-pid-taskinitfunction ,your-pid_Task , NULL, 20, GT_RUNABLE_NULL, GT_INTERNAL_NULL, (void *)&GT_AllTasks[2]},
/*Type Activation Type Id Prio init Function Task Function TaskArg WL/Runable Internal External */
{ GT_TASK_EXT , GT_ACT_INT , 0, 32, your-timer-taskinitfunction ,your-timer_Task , NULL, GT_RUNABLE_NULL, GT_INTERNAL_NULL, (void *)&GT_AllTasks[0]},
{ GT_TASK_EXT , GT_ACT_INT , 1, 31, your-imu-taskinitfunction ,your-imu_Task , NULL, GT_RUNABLE_NULL, GT_INTERNAL_NULL, (void *)&GT_AllTasks[1]},
{ GT_TASK_EXT , GT_ACT_INT , 2, 33, your-pid-taskinitfunction ,your-pid_Task , NULL, GT_RUNABLE_NULL, GT_INTERNAL_NULL, (void *)&GT_AllTasks[2]},
//{ GT_TASK_DEF , GT_ACT_INT , 3, 33, _GT_GetNextActivation, _GT_GetTaskCet, NULL ,NULL , NULL, 20, GT_RUNABLE_NULL, GT_INTERNAL_NULL, (void *)&GT_AllTasks[3]},
};
void GT_T00_Start(void){
asm volatile("nop");
}
@@ -125,10 +72,6 @@ void GT_T02_Stop(void){
}
/*
* start and end hooks
*/
void GT_TaskStartHook(GT_TASK_T *pMyOwnTask) {
switch(pMyOwnTask->Id){
case 0: GT_T00_Start(); break;

75
src/APP/Aufgabe7/ps7/core0/src/pid.c
View File

@@ -13,6 +13,79 @@
#include <stdlib.h>
#include <math.h>
#include "pid.h"
#include "imu.h"
//insert your pid-code from task 4
#define KP 300.0
#define KD 0.0
#define KI 200.0
#define SAMPLE_TIME ((float)(5.0/1000.0))
extern float imu_current_angle;
static float targetAngle = 0.0;
int32_t pidValue=0;
float errorSumAngle=0;
float prevAngle=0;
int cycle = 0;
#define ARRAY_SIZE 1000
int32_t pidArray[ARRAY_SIZE];
int arrayIndex = 0;
/* * * * * * * * calculates a PID value from -1000 to 1000 by using acc and gyro data * * * * * * * * */
int pid_Task(void* pdata) {
float errorAngle=0;
CPU_SR cpu_sr;
float currentAngle;
cycle++;
if (cycle == 10 * 1000/5)
targetAngle = prevAngle;
OS_ENTER_CRITICAL();
currentAngle = imu_current_angle;
OS_EXIT_CRITICAL();
errorAngle = currentAngle - targetAngle; //calc the error to the disired angle
errorSumAngle = errorSumAngle + errorAngle; //integrate the error to use it in the PID
errorSumAngle = (float)CONSTRAIN((int)errorSumAngle, -750, 750); //constrain maximum integrated error
pidValue = (int16_t)(KP*(errorAngle) + KI*(errorSumAngle)*SAMPLE_TIME + KD/SAMPLE_TIME*(currentAngle-prevAngle)); //calculate output from P, I and D values
pidValue = 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;
}
prevAngle = currentAngle; //prepare next iteration
pidArray[arrayIndex] = pidValue;
arrayIndex = (arrayIndex + 1) % ARRAY_SIZE;
return 0;
}
/* * * * * * * * clears Gyro offset and sets it new * * * * * * * * */
int pid_Init(void *pdata){
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;
}